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ABSTRACT 


A semi-autonomous vehicle, MONTe, was designed, modeled and tested for deployment and 
operation in a surf-zone coastal environment. The MONTe platform was designed to use unique 
land based locomotion that incorporates wheel-legs(Whegs™) and a tail. Semi-autonomy was 
realized with data from onboard sensors and implemented through open source Robot Oper¬ 
ating System (ROS), hosted on an Ubuntu Linux based processor. Communications via IEEE 
802.11 protocols proved successful for data telemetry in line of site operations. Basic mobility 
and tail control of the platform was modeled in Working Model 2D. Eield tests were success¬ 
fully conducted to demonstrate mobility and semi-autonomous waypoint navigation. Euture 
developments will look to improve the overall design and test water borne mobility, navigation, 
and communication. 
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CHAPTER 1: 
Introduction 


For nearly a decade, the Naval Postgraduate School (NPS), Case Western University and ad¬ 
ditional collaborators have been involved in a program to develop an autonomous surf-zone 
robot. In general, unmanned systems provide significant advantages for military operations and 
commercial applications. Autonomous and remotely operated systems provide advanced ca¬ 
pabilities at a lower cost, while not placing personnel in harm’s way. There have been recent 
advancements in unmanned systems that operate solely in one mode, such as land based or wa¬ 
ter based. Overcoming the transition from one mode to another, such as an amphibious robot, 
still requires significant research. The environmental conditions and duality associated with the 
surf-zone presents a unique set of challenges. There have been several previous platforms that 
have attempted to address the difficulties associated with these harsh conditions. Some of these 
platforms have provided invaluable insight into complex mobility and autonomy. 

Interest for these types of systems that operate in coastal areas and shallow beaches is widespread 
Platforms wielding these capabilities can be outfitted with a multitude of sensors to accomplish 
a spectrum of missions. Tasking may include minesweeping or clearance, terrain or bathymetry 
surveys, covert reconnaissance and surveillance. As sensor packages become more compact, it 
is possible to envision equipping such a robot with a chemical detection unit that may be able to 
search for specific compounds or chemical weapons. The versatility of these platforms justifies 
the need to research and develop a robust surf-zone robot. 

1.1 Background 

1.1.1 Previous Designs 

Whegs™ (wheel-legs) describes a class of robot that characterizes its locomotion based on a 
fusion of a wheel design with crawling leg that was inspired by the motion of biological organ¬ 
isms. Case Western Reserve University’s Biologically Inspired Robotics Laboratory developed 
this means of locomotion under Roger Quinn [6]. The idea was based on the high maneuver¬ 
ability of a cockroach and its ability to overcome adverse obstacles. This design concept has 
been incorporated into several versions of surf-zone robots. 

An earlier Whegs™ design was the Dayton Area Graduate Studies Institute (DAGSI) Whegs™ 
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Figure 1.1: From [1], a picture of Agbot 


prototype called Agbot. NPS and Case Western Reserve University collaborated to build Agbot, 
pictured in Figure 1.1. Agbot’s design consists of six Whegs™, a single section body, and is 
steered by angling the front Whegs™, similar to an automobile. The autonomy was limited to 
waypoint navigation from a control station. Agbot’s main purpose was to act as a test platform 
for the mobility of the Whegs™ and their ability to overcome obstacles. Detailed information 
regarding Agbot is available in [1]. 

Variants of Agbot have been produced, which have consistent designs and been subject to ad¬ 
ditional testing [2]. Although success has been shown for climbing large obstacles, improved 
designs have the potential for significant advances. One proposed improvement is replacing the 
rear segment of the main body with an autonomous tail. The main motivation for incorporating 
a tail into a surf-zone robot is to climb larger obstacles and terrain [2]. This modeling was done 
in Working Model 2D and still needed to be verified with prototype trials. The addition of a tail 
and removal of two Whegs™ also implied that the stability would be improved (discussed in 
Section 2.1.1) with a new design comprising four legs per Wheg™ instead of three. A concep¬ 
tual rendering of a next generation robot is seen in Figure 1.2. For a more detailed explanation 
of these mobility concepts see Section 2.1. 

ROBSTER, Figure 1.3, was developed to serve as a test platform for the addition of a tail [3]. 
This initial investigation into tail control was conducted by Courtney Holland at NPS in June 
2009. ROBSTER’s design incorporated the new Whegs™ style and consisted of a rigid tail that 
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Figure 1.2: From [2], a 3D rendering of a future design calied Peiican Whegs™ 



Figure 1.3: From [3], a picture of Robster 


was 2/3 the length of the entire robot. The torque of the motor and gearing allowed the tail to lift 
the entire rear of the robot. Dynamic tests were conducted to determine the effects of using the 
tail for climbing assistance. These tests did not create a high centering scenario that the design 
is susceptible to. Some recommended design improvements reported by Holland included the 
incorporation of a solid-state micro-electro-mechanical systems (MEMS) inclinometer and an 
improved control algorithm that reduces spurious sensor data. 
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Dunbar developed navigation and eontrol for Agbot in his thesis, ineluding an effeetive way- 
point navigation algorithm that interfaeed with a Java based graphieal user interfaee (GUI), 
written by Uzun, for a robot named Bender [2]. Agbot eould navigate up to 10 waypoints, and 
report eurrent GPS position and heading. Williamson wrote a proportional, integral, deriva¬ 
tive (PID) eontrol seheme to eontrol the motors and ealibrated the appropriate gains for an 
autonomous ground vehicle named Bigfoot [7]. 

1.1.2 Related Works 



Figure 1.4: From [4], a 3D rendering of AQUA outfitted with walking legs 


There are a variety of research groups that have studied platforms designed for autonomy and 
some use specialized methods for mobility. Select robots have also been designed for amphibi¬ 
ous operations. AQUA is an advanced robot that has two modes of operation, the land based 
version in Figure 1.4 and a waterborne variant that uses flippers instead of legs [4]. The vehi¬ 
cle’s means of locomotion are unique and together attempt to overcome the difficult transition 
from water to land. The Surf Zone Crawler Group from the Naval Surface Warfare Center has 
also focused on this operating environment in order to provide mine detection and classification 
[8]. Their group proposed that different unmanned systems work as a team to perform separate 
tasks. Another design seen in Figure 1.5, called AmphiRobot, uses a unique tail and propeller 
system to provide mobility in water and land [5]. This robot features a variety of sensors and 
servomotors that allow for object avoidance and additional autonomy. Commercial teams have 
also developed crawling robots that can be used to conduct non-destructive testing inside oil 
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and chemical tanks [9]. In order to deliver accurate inspection results these adverse constraints 
have lead to interesting designs that require advanced sensors and control systems. 



Figure 1.5: From [5], a 3D rendering of AmphiRobot 


Controls and robotic operating systems are active areas of research in robotics. Autonomously 
integrating sensor data to navigate and perform tasks in an unmanned vehicle is a big ticket item 
for the Defense Department and the private sector. Numerous institutions and agencies, such 
as the Defense Advanced Research Projects Agency (DARPA) and (AUVSI) have sponsored 
many competitions and research projects in the subject. One example is the DARPA Urban 
Challenge, the last of which was held in 2007. Teams had to convert automobiles to negotiate a 
complex course in an urban environment autonomously with no human assistance [10]. 

The operating systems and architectural structures for controlling robots are many and varied. 
Previous work was done using DynamicC©. Developed for use with Rabbit Microprocessors, 
Dynamic C(c)provides multitasking capability for a robotic project. Lopez, Bigfoot, and Agbot 
programs were all written in Dynamic C. The Robotic Operating System (ROS) is another 
example of an object oriented operating system, which is open source and maintained by Willow 
Garage. Programs like ROS allow for rapid prototyping and integration of software. Examples 
include the AsTec Quadrotor unmanned aerial vehicle, Clearpath Kingfisher sea-based system, 
and the iRobot Roomba [11, 12]. 
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1.2 Concept of Operations 

An autonomous amphibious vehicle that could operate in the surf-zone would provide a valuable 
capability to the military. An inexpensive robotic platform could be deployed covertly from the 
sea and make its way onto the shore and replace the need to send in a human. This could be 
accomplished on the surface, or subsurface depending on the system. 

The transition from the sea to shore is the unique aspect of this concept. Crashing waves, rock 
formations, and other features provide quite the challenge in negotiating its way to shore. A 
surf-zone robot would need to utilize multiple systems to successfully make this transition. 

Once ashore, the platform can perform reconnaissance, disable mines, or deploy devices de¬ 
pending on the mission requirements. This is only a short list of the capabilities provided by 
this kind of vehicle. The sensor and mission packages could be modular to provide flexibil¬ 
ity. Multiple sensor inputs, including GPS, inertial navigation systems (INS) and stereovision, 
provide positional and path-finding capabilities. 

Communication both at sea and ashore will need to be handled effectively. Wireless or laser 
point-to-point communications will work well for the surface. Submerged navigation and com¬ 
munications can be handled via acoustic beacons like Seaweb [13]. The concept can be taken 
further where a “mother ship” style deployment system can be implemented. An offshore plat¬ 
form can deploy and serve as the communications hub for the surf-zone robots. Once the mis¬ 
sion has been accomplished the robots can transition from shore to sea for scuttling or recovery. 
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CHAPTER 2: 

Concepts for Mobility and Navigation 


2.1 Mobility 

2.1.1 The Use of Whegs™ 



Figure 2.1: From [1], two graphic representations of Wheg™designs where left is four spoke variant and right is 
three spoke variant 

The Wheg™ has been an instrumental aspeet in the design of a surf-zone robot. It allows for 
fast travel over smooth terrain and offers the ability to climb over obstacles. Figure 2.1 shows 
a basic diagram of two different variations of Wheg™ design. The three spoke variant was 
originally introduced on a robot with six Whegs™. Their rotation was phased in a manner to 
always have three Whegs™ in contact with the ground. This provided sufficient stability during 
locomotion and while standing still. The three spoke variant is also able to climb a higher step 
size when compared to a four spoke variant. The one disadvantage to having three spokes is 
that the center of rotation has a large vertical variation as the Wheg^“ rotates and advances the 
position of the Wheg™. This produces significant vibrations as the robot moves along a path. 
Having groups of Whegs™ phased together reduces the overall undulation of the robot. 

The four spoke variant helps limit the vibration and vertical variation seen at the center of the 
Wheg™. The step height geometry in Figure 2.1 corresponds to a 30% vertical variation for 
four spokes vice 50% for three spokes. More spokes offer more stability since there is one 
additional leg to provide support through one rotation. Ideally a complete wheel would be used 
to limit the vibration, however that would sacrifice the ability to climb obstacles and travel in 
complex terrain such as loose sand. The four spoke variation does limit the step size that it is 
able to climb over; however it is only a six percent reduction. Overall the Wheg™ has proven 
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successful for surf-zone operations and different variants ean be used for specifie applieations 

[ 1 ]. 


2.1.2 Optimizing the Center of Mass 



Figure 2.2: From [2]: Left image shows a previous generation robot successfui climbing an obstacle in modeling 
environment, Center picture shows accompiishing the task by a prototype robot, Right image shows a theoretical 
design with a taii capabie of ciimbing a higher obstacie 


Previous generations of Wheg™ robots used an overall symmetric design eonsisting of a front 
and rear segment. These designs, one of whieh is shown in Figure 2.2, have had significant 
success at elimbing large obstaeles. Replaeing the rear segment of the artieulating body with a 
tail shifts the eenter of mass forward on the robot allowing it to elimb 20 precent larger obstacles 
[2]. Video from various trials was earefully reviewed and it was determined that the loeation 
of the eenter of mass relative to the position of the eenter Wheg™ was the deeiding faetor for 
success. The tail acts as a support point behind the main body that can provide leverage to lift the 
rear of robot. The ineorporation of the tail optimizes the center of mass while elimbing adverse 
terrain and increases the overall mobility of a surf-zone robot. Further rigid body analysis and 
modeling should be investigated to determine more quantitative insight into this high center 
seenario. 

2.2 Navigation 

2.2.1 Waypoint Navigation 

Path planning using GPS waypoints is a simple but effective means of navigating a robot through 
its environment. In this mode, the robot will find the heading to the destination and drive towards 
it. Sinee the destination is set by the user, autonomy in this mode is limited to driving the plant 
to arrive at the destination. 

For this method of implementation, obstaele avoidanee is possible only through the user’s se- 
leetion of a safe path. Implementation of objeet avoidance will require additional sensor input. 
Additional sensors ean be used to implement more sophistieated navigational methods sueh 
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Figure 2.3: Example of PID control loop 


■Measure mept 


as “bug” algorithms (that skirt around the edges of eneountered obstacles) and potential field 
navigation [14]. Advanced path planning techniques will be investigated in future works. 

2.2.2 Plant Control 

A robot using a compass and GPS waypoints needs to have a feedback control to navigate 
successfully to its destination. Proportional, Integral, and Derivative feedback control (PID) is 
a popular method. PID control provides effective system response capabilities and tuning of 
system parameters [15, 16]. 

Figure 2.3 illustrates the function of the PID feedback loop that can be used with heading 
control. First, the desired heading is differenced with the current heading and the error signal 
is produced. This error, denoted e{t), is then fed into the PID compensator which consists of a 
series of gains denoted Kp, Ki, and K^, such that the control signal is given by: 

u{t) = Kpe{t) + Ki ^ [ e{T)dT + (2.1) 

-Li Jo dt 

Equation 2.1 shows the time response of a typical PID controller. The proportional gain, Kp, 
is a gain that is applied to the error to provide a signal. This gives the kick to the plant to get 
it traveling towards the destination, but may lead to “hunting.” Hunting is where the robot’s 
heading oscillates about the desired path. This typically happens when the gain is too high 
and is known as under-damping. If Kp is too low the response will take a long time to reach 
the ordered heading (i.e. over-damped), and could never reach the waypoint depending on the 
positional geometry. 

Integral control, K^, helps solve some of these issues. The integrator sums the error over a 
given period of time, T/, and then applies the resulting gain to the plant signal. This allows 
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the feedback loop to correct for encountered friction and inertia that produces a response offset. 
The error is time-averaged, and will give a boost to the signal if the outcome is slow to respond. 

The final component is the derivative control, Kd, which helps improve dynamic response. 
This method monitors the rate of change in the error, over time To, which can help minimize 
overshoot of the desired outcome. 

The signal generated by the PID control is then fed into the plant which produces a response. 
The feedback (current heading in this case) is then fed back into the loop and the process starts 
again. All, or some, of the components can be implemented depending on the desired response 
required. 

2.2.3 Gain Scheduling 

When implementing PID control, selecting the appropriate gains is vital. This does not mean 
Kp, Ki, and Kd need to be constant. This is commonly referred to as gain scheduling. One im¬ 
plementation is to change Kp, Ki, and Kd based off of the current terrain the robot is traversing. 
Another use is to limit the maximum gain until a threshold is reached. For example, a robot 
could switch off PID control if the heading error was greater than 90 degrees. If outside this 
tolerance, the robot would simply turn at the maximum rate until the error dropped below the 
appropriate level. [16] 

A more detailed discussion of PID control can be found in the thesis written by Dunbar [1]. 
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CHAPTER 3: 
Design 


The first step in creating Mobility Over Non-trivial Terrain (MONTe) is to dissect the concept 
of operations and determine characteristics that will be inherent to our design. For example, an 
amphibious robot could be designed to crawl on the bottom of a body of water, float at the sur¬ 
face, or possibly be engineered to do both. These preliminary design constraints establish core 
capabilities that act as a foundation for the overall design. Our team placed three constraints on 
the MONTe. First, the robot is watertight, vice free flood, and is positively buoyant. Designing 
the robot to float on the surface improves the reliability of communications, allows for the re¬ 
ception of GPS information, and simplifies the initial testing environment. Secondly, Whegs™ 
provide MONTe’s land based locomotion and a tail assists in climbing terrain. Lastly, the robot 
is semi-autonomous vice tethered. These constraints may be altered for later builds as MONTe 
matures through testing and improvements. 

The following provides a quick overview of the design, referencing Figures 3.1 and 3.2. For 
details on the mechanical design, refer to Slatt [17]. 


Left/Right 
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Radial Arm 
Assembly 


Main Body 




Figure 3.2: Top view of MONTe’s components 


3.1 Mechanical Components 

3.1.1 Main Body 

The challenge of making MONTe watertight led to the selection of a Pelican Case for the main 
body. This allows for the main access to be sealed with a gasket while allowing frequent opening 
and closing throughout testing. As additional penetrations are made through the case, they are 
sealed individually using gaskets or techniques. 


3.1.2 Drive Assembly 



Figure 3.3: Picture of MONTe's drive assembly attached to half of a radial arm 


Attached to both sides of the main body are the left and right drive assemblies. These drive 
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assemblies eontain the drive motor shafts that eonneet to a main drive belt. It also houses a 
suspension system for both the front and rear Whegs™. Limit switehes are triggered if any 
force displaces this suspension from its equilibrium position, which is used to detect a free 
rotating Wheg™. The drive assembly can be seen in Figure 3.3. 

3.1.3 Radial Arm Assembly 

The radial arm assemblies connect each Wheg^“ to its associated drive assembly. Each assem¬ 
bly contains a belt and pulley configuration that transfers torque from the drive assembly to 
each Wheg™. It comprises two halves that are joined together. One of these halves can be seen 
in Figure 3.3. 

3.1.4 Wheg™ 



Figure 3.4: Picture of MONTe’s initiai Wheg™design 


This Wheg™ design, Figure 3.4, is a four spoke variant. As discussed in Section 2.1.1, it 
offers less vibration over smooth terrain, but limits the step size for which the Wheg™ will 
climb. There has also been a decrease in the total number of Whegs™from six to four. The 
Whegs™ are designed to be easily removed and allows for quick implementation of changes to 
the Wheg^'^ design. The shape and symmetry of the Wheg™ affect its ability to climb and will 
continue to be adapted throughout the development process. 


13 





3.1.5 Water Jets 



Figure 3.5: Picture of MONTe’s future water jet, ieft image shows the piacement and right image shows the compo¬ 
nents 

Water-borne loeomotion will be provided with a water jet system, as shown in Figure 3.5. The 
motors, ducting, and impeller will be housed in the main body. The ducting acts as a new 
pressure boundary within the main body. This allows the jet motors to be isolated from the 
water while providing thrust. 

3.1.6 Tail 

One of the major advancements of MONTe is the incorporation of an autonomous tail. The 
tail was introduced to the overall design in order to overcome a susceptibility to high centering 
by moving the center of mass forward. The tail is designed to assist in climbing obstacles and 
to self-right the robot in the event it becomes flipped over. Tail operation is modeled for to 
determine performance, which will be discussed in the Section 4.3. 

Mechanical Design of Tail 

The tail for MONTe is relatively simple in construction and provides the mechanics for an initial 
proof of concept. The most apparent simple design is a rigid flap that attaches to the robot at 
a joint as seen conceptually in Figure 1.2. Two independent joints provide rotational motion 
along the same axis. To prevent the tail from obscuring any sensors, a “wire frame” structure is 
used. Figure 3.6. Additionally, it reduces the overall weight of the tail and maintains a similar 
level of strength. 

Steel tube, 3/8” outer diameter with a 1/16” wall thickness provides the main structural support 
to create the tail. When in a stowed position. Figure 3.7, the tail wraps around the main body 
case of MONTe, as in Figure 3.6. The frame is supported by a cross member that bends over the 
top of the case. The cross member is placed in an optimal location. It is positioned close to the 
control joints to provide support while ensuring enough distance to prevent interference with 
components when in the extended position. The tip of the tail is left hollow to allow extensions 


14 








Figure 3.6: A picture of MONTe with the taii in the stowed position 



Figure 3.7: MONTe's taii positions (a) Down (b) Stowed (c) Neutrai 


to be inserted into the tubes. These extensions allow for changing the overall length of the tail 
as necessary throughout design and testing. 

The tail is 11.8 ounces and is attached to a high torque servo drive mechanism. These servo 
components are attached to the drive assemblies using steel brackets and fasteners. The com¬ 
bined bracket and servo drive mechanism weighs 16.2 ounces. Later designs will reduce the 
overall weight by replacing the steel bracket with lighter weight polycarbonate materials. Com- 
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billing the tail bracket with the drive assembly will eliminate fasteners and be a necessary design 
improvement to maintain watertight integrity of the overall design. This single unit will encase 
the servo drive gears shielding it from debris and sand and thus preventing damage to the gears. 



Figure 3.8: A picture of the high torque servo drive mechanism 

Tail Drive Mechanism Design 

The tail drive mechanism consists of a titanium geared hobby servo, HS-7955TG, and a supple¬ 
mental gearbox, Figure 3.8. The stock servos were modified to allow continuous rotation of the 
servo and incorporate a new potentiometer into the gear train. These high torque servos provide 
95-3300 oz-in of torque [18]. 



Figure 3.9: A graphic showing a simpie iever arm that can be used to estimate the required torque output of the 
servo drive mechanism 


Torque = r X F |Torque| = |r| |F| sin 0 = /te (3.1) 
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Preliminary calculations were necessary to provide an estimate of the torque required to operate 
the tail. The general torque expression is given by Equation 3.1, where F is the applied force 
vector and r is the displacement vector from the joint to the applied force. Based on a weight 
estimate of the entire robot and the location of the tail joint, that equation reduces to a simple 
expression. Figure 3.9 depicts the moment arm that transmits the torque of the center of mass 
on the tail joint. Based on an anticipated final weight (w) of 20 lbs. and a moment arm (/) of 8 
in., the estimated total torque would be 2600 oz-in. Since two servo mechanisms will be used 
to drive the tail, each unit would need to supply roughly 1300 oz-in, which is governed by the 
following equations [19]: 


Tm = (3.2) 

T = Jeq9 + Feq9 (3.3) 

Jeq = Ja~\~ L (3.4) 

Feq = Fa+ (3.5) 


As seen in Equation 3.2, torque of the electric tail motor is proportional to the current that it 
draws, /«, flux for each pole, 0, and a constant, Kt, related to the physical design of the motor. 
This is a design issue for MONTe since current is provided from a limited battery system, 
see Section 3.4. Even if ample current is available, electric motors will stall if the combined 
friction and inertia applied to the motor are too great. The mechanical equations of motion for 
the joint are given by Equation 3.3. This shows that torque is proportional to the inertia, J^q, and 
friction, F^q, through either the angular acceleration, 9, or angular rate, 9. When considering a 
joint driven by an electric motor, the inertia and friction can be divided into two components, the 
armature of the motor, a, and the external load, L, of Equations 3.4 and 3.5. When considering 
MONTe’s self-righting high torque scenario, the load produces an overwhelming effect over 
the armature. By applying a mechanical gear, the torque is transmitted from a longer moment 
arm and acts to reduce the inertial and frictional effect on the motor. Equations 3.4 and 3.5 also 
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show that by selecting the proper gear ratio, N, the inertial and load can be dominated by the 
armature and prevent the motor from stalling. The tradeoff is that the tail rotation speed will 
be reduced. Testing in Section 4.3.1 further investigates the torque required for MONTe’s servo 
drive mechanism. 

3.2 Operating System Architecture 

Robots designed for reconnaissance must be capable of several tasks if they are to be useful 
to the organization that operates them. A robot must first be able to get to the target location. 
Once there, it must be able to orient itself in its environment and the region of interest. Its 
sensor package must be able to find and record data of interest. Finally, the robot must be able 
to communicate at some point with the operator in order to complete its mission. 

The level of autonomy becomes a vital part the design implementation. At one end of the spec¬ 
trum is an unmanned vehicle that is fully user-operated. While easier to design in a technical 
sense, this can be prohibitive in the man-hours required to operate it. Furthermore, the user 
will most likely be only able to operate one unmanned system at a time. The other end of the 
spectrum is a system designed to require no user input beyond mission parameters. 

MONTe is designed to operate in the middle of the spectrum. To be semi-autonomous in nature, 
MONTe can communicate, navigate and be controlled by the operator as the situation warrants. 

The primary operating system for MONTe is located on the LPC-lOO computer. The main 
program’s responsibilities include communications, navigation, plant control and eventually 
stereovision. The architecture of the operating system is based off the Robot Operating System 
(ROS). 

3.2.1 ROS Overview 

ROS is an open-source, meta-operating system designed for robotic applications. It uses a peer- 
to-peer network messaging system between different processes. The different processes are 
loosely coupled by being compartmentalized. It is not a real time operating system. ROS is also 
designed to allow for portability between different robotic projects [11]. 

Fundamentally, ROS operates as an object-oriented messaging service. This allows commu¬ 
nications between different processes. These processes are called nodes in ROS, which are 
nothing more than software code and drivers. This provides a great framework for incorporat¬ 
ing sensors and other devices into the robot. Incorporating a laser rangefinder involves writing 
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a driver to interface with the device and then transmit the information to other nodes that need 
the information. 

The first mode of communications is a traditional service style messaging system. One node 
waits for a signal from another node before transmitting a response. This provider/client system 
is effective but requires the nodes to be more coupled than is desired for this project. 



Figure 3.10: Illustration of a basic ROS publisher/subscriber interaction. 


MONTe uses a publisher/subscriber framework for handling inter-nodal communications. The 
framework is made up of publisher nodes, subscriber nodes, and topics. Figure 3.10 provides a 
basic illustration of the interaction of nodes through a topic. The publisher generates a message 
(described by a user-defined .msg template) that is then published to a topic. The subscriber 
will read from the topic, via another message, by a polling process called “spinning”. The 
publisher and subscriber are decoupled because neither directly sees the other. Taking the pub¬ 
lisher/subscriber concept further, multiple nodes can subscribe or publish to the same topic. A 
node has the capability to both publish and subscribe. This allows for complex interactions 
between nodes, and the decoupling makes debugging nodes easier [20]. 

3.2.2 Functional Architecture 

MONTe’s program has several design goals for this version. Conceptually, the architecture 
needs to provide a foundation for current and future work. Physically, it needs to interface with 
sensors, control the motors, and communicate with the operator. Behaviorally, it needs to fuse 
the sensor data and user inputs to navigate effectively. Finally, the structure of the program 
needs to be de-conflicted so that the nodes are not interfering. 

Figure 3.11 outlines the basic structure of MONTe’s main program. It illustrates all nodes, 
topics, and hardware interfaces. It further illustrates the flow of information and commands 
through the network of nodes. A more detailed discussion of the individual nodes and topics 
will follow. 
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Figure 3.11: Diagram of MONTe ROS Architecture 
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ROS Nodes 

Nodes are vital to how ROS operates. Fundamentally, nodes in ROS are funetions that perform 
tasks that an autonomous system needs to aceomplish. Messages and topies allow eaeh node to 
operate independently, as eaeh only sees the topics it is subscribed to. 

Figure 3.12 shows typical initialization commands used in a ROS node. Upon launch, each 
node will invoke ros : : init and ros: :NodeHandle to initiali z e the node and provide a 
name or “handle” for ROScore to interact with. Next, all publishers and subscribers are set 
up via the advertise/subscribe functions. Finally, the messages needed to talk to the topics are 
initialized. In this case, MONTe : : PlantXommand cmd initializes a message handle cmd 
of message type Plant_Command.msg. This would enable messages to be sent to the ROS 
topic Plant_Command_T. 

At this point the node will enter a loop to perform calculations. The first or last item in the loop 
should be to “spin” ROS in order to poll all topics, ros : : spinOnce () polls any callback 
functions that have been declared in the node. The node can also publish at any point of the 
loop using the . publish () command. 
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/* Sample code to demonstrate ROS concepts */ 

int main ( void ); 

{ 

ros :: i n i t ( argc , argv , ”MONTe_Navigation” ); // Set up ROS node 

ros :: NodeHandle n; // Set up handle for this node 
// Set up all publishers for node 

ros :: Publisher plt.cmd.pub = n . adverti se <MONTe: : Plant.Command >(”Plant_Command_T” , 1); 

// Set up all subscrivers for node 

ros :: Subscriber sub.cf = n . subscribe (”Command_Flags_T” , 1, Command.FlagsCallback ); 

// Set up message handles 
MONTe:: Plant.Command cmd; 

MONTe:: Command.Flags flags ; 

/* More code here */ 
while ( ros : : ok ()) 

{ 

ros : : spinOnce (); // Poll topics 

/* Perform calculations */ 

plt.cmd.pub . publish (cmd); // Publish data to topic 

} 

} 

void Command_FlagsCallback ( const MONTe:: Command_FlagsConstPtr& flags) 

{ 

Cmd.Flags . autonav = flags—> auto.nav ; 

Cmd.Flags . mode = flags—> nav.mode ; 

Cmd.Flags . route = flags—> incoming.route ; 

} // end callback 


Figure 3.12: Sample code illustrating ROS concepts 

MONTe’s nodes can be conceptually divided into two types: demand and continuous. The 
demand nodes consist of the Waypoint Control and Keyboard Control. Thesedonot 
run in a continuous loop, instead wait for input prior to executing. The other nodes are designed 
to be running in a continuous loop. These nodes will run processes, and poll and publish to topic 
at a rate of 4 Hz. The nodes do this by taking advantage of the ROS loop.ratesleep () 
function that enforces the desired frequency. The system cycle rate of 4Hz was selected based 
on updating the navigation algorithm at a sufficient rate, but is open to further optimization. 

Communications Node 

This node covers the communication with the base station and operator. The node communi¬ 
cates via UDP protocol over a series of ports to keep data streams separate. The communication 
types are divided into a series of channels. Each channel, upon receipt or transmission, will 
publish data and control flags to various topics. An example would be manual control. Upon 
receipt of a manual command, the Communications node would process the input and pub¬ 
lish the pertinent data to ROS topics l ik e Plant.Control topic and Command_Flags . 
The other communication channels include Waypoints (receive). Navigation (send), and 
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Message (send). 


Waypoints Node 

The Waypoint node proeesses operator generated navigation data. The waypoints are stored 
in the node using dynamieally alloeated memory, and are arbitrarily limited to ten waypoints. 
The staek has funetionality to switch between waypoints, create more and to delete the stack. 


Waypoints node subscribes to the New_Waypoint and Command_Flags topics to manip¬ 
ulate and input new waypoints into the stack. The node publishes the current destination to the 

Current.Waypoint s topic. 



Figure 3.13: Flowchart for MONTe’s Navigation Node 


22 











































Navigation Node 

The Navigation node is theprimary behavior generator for MONTe. In Figure 3.11, Navigation 
resides at the eenter to represent its importance in the overall architecture. All major data paths 
begin or end with the navigation node. 

The current program will allow MONTe to travel to a desired waypoint without object avoid¬ 
ance. Figure 3.13 details the operation of the navigation routine. Navigation polls Command_Flags 
and Current.Waypoints to verify MONTe is in auto-nav mode while updating the next 
destination. Positional data (compass and GPS) is received from the Nav_Data topic. Cur¬ 
rent position and the current waypoint are compared and appropriate plant control data is pub¬ 
lished to Plant-Control topic. When the current destination is reached a flag is updated on 
Command-Flags so that the Waypoint node can send the next waypoint. 

Monkey Node 

The Monkey node is the driver associated with the Monkey Attitude Heading Reference System 
(AHRS) unit. In the current implementation, the node is a publisher only. Its primary function 
is to receive GPS, compass, and velocity data for publishing to the Nav-Data topic. In the 
future, its secondary function is to allow for manual control of the tail or adjust the autonomous 
functions of the tail. 

Plant Control Node 

The Plant Control node is simple in operation. It is the driver for interfacing with the 
Sabertooth2xl2 motor drivers. It handles this by polling the Plant-Control topic to receive 
commands. They are then parsed and transmitted via RS-232 serial port to the motor drivers. 

Keyboard Control Node 

Keyboard Control allows the user to control MONTe manually via a virtual network client 
(VNC) server. Control is rudimentary with forward, reverse, stop, left, and right turns possi¬ 
ble. It also allows the speeds and turning rates to be adjusted during operation. Upon receipt 
of a command, the node will publish the motor commands to the Plant-Control topic, 
while updating the Command-Flags topic to manual control. This will take MONTe out of 
autonomous navigation. 

Waypoint Control Node 

Waypoint Control allows the user to input and delete waypoints, and then execute the 
route remotely via VNC. The maximum number of waypoints is set to ten. Each waypoint is 
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# Plant.Command . 

4+ 

msg 






ft 

# Basic message 

for 

manually controllin 

g MONTe in simplified serial mode 


# Speed command 
uintS left 

for 

left 

motor . 

Range 

is l(Full Reverse)—> 64 (Stop) <— 

127 (Full Forward) 

# Speed command 
uintS right 

for 

left 

motor . 

Range 

is 128(Full Reverse)—> 192 (Stop) 

<— 255 (Full Forward) 


Figure 3.14: Example of a ROS message 


set up with latitude, longitude (in deeimal degrees), waypoint number, and an aetion. Aetions 
are set up for future use for more sophistieated eontrol. Onee a route is generated, the user will 
send the route whieh feeds into the New.Waypoint topie. Command_Flags topie will also 
be updated to autonomous navigation whieh will take MONTe out of manual eontrol. 

ROS Topics 

Topies allow ROS to transmit data between nodes. The nodes only see the topies they publish 
and subseribe to, whieh deeouples them. Eaeh topie is eommunieated to and from via a message. 
Valid data types inelude integers, floating point, eharaeters, and strings. A sample message is 
shown in Figure 3.14. This simple message holds to unsigned, 8-bit integers, and ean be used 
to send to or reeeive from a topie. 

The main type of topies that MONTe utilizes transfer sensor data between the nodes. For 
example, Nav_Data provides the eurrent position and heading of MONTe to any node that 
requires that data. Other messages transfer eommand data to topies sueh as New.Waypoint 
and Current_Waypoint. The messages in this ease send waypoints with any additional 
information required for proeessing the data. 

Command_Flags topie is a speeial topie that stores all behavioral flags that eontrol MONTe’s 
operation. The eurrent flags it stores are auto.nav, navjnode, and incoming_route. 
These flags allow MONTe to switch between manual and autonomous control modes, indicate 
when a waypoint is reached, and to warn when a new waypoint route is in the queue. 

3.3 Control System Hardware 

The control system hardware is housed in the main body of MONTe, shown in Figure 3.15. 
Individual components are mounted onto a power module that acts to support the electronics 
and route wiring for the devices. This assembly also organizes the switches that activate each 
power bus and device. 
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Figure 3.15: Picture of the internal design and component placement of MONTe 


3.3.1 Main Processor 



Figure 3.16: Picture of the Stealth LPC-100 

The main computational device for MONTe is a Stealth LPC-100 mini-personal computer, Fig¬ 
ure 3.16. It was selected due to its small size. It weighs 1.2 lbs, and its dimensions are 4.0”(W) 
X 6.1”(D) X 1.45”(H). This small form factor is ideal for use in a self-contained robot. 

The computer runs on a 1.9GHz Intel-Celeron processor with 4GB of RAM. This provides a 
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robust capability for running the operational program, as well as serving as the eommunieations 
hub. The operating system is Linux/Ubuntu 10.04 that allows a stable version of ROS to be run. 

3.3.2 Monkey Board 



Figure 3.17: Picture of the 2010 Monkey Board produced by Ryanmechatronics LLC 

The Monkey 2010 platform, Figure 3.17, is a versatile cireuit assembly that can be used to 
control autonomous vehicles. There are multiple eapabilities inherent to the board, but some 
of the main funetions inelude: a Cortex M3 (ARM 7) proeessor, U-Blox NEO-5 GPS module, 
barometrie pressure sensor, FO ports, pulse width modulation (PWM) servo outputs, and status 
LEDs. This board is designed to accompany a CHIMU (product name) AHRS. A robust soft¬ 
ware suite allows for easy user interfaee and eontrol algorithm development. Combining these 
modules allows MONTe to acquire its GPS position and know its spatial orientation: roll, pitch, 
and yaw. The Monkey is limited in its video processing eapabilities and therefore only used for 
advaneed sensing and tail eontrol. 

3.3.3 Motor Driver 

The Sabertooth2xl2, shown in Eigure 3.18, is a 6-24V motor driver designed for analog, radio 
eontrol (RC) and serial eontrol applieations. It ean operate at up to 12A eontinuously and eontrol 
two sets of motors per channel. Serial control in simplified mode was seleeted for eontrolling 
MONTe. Single byte eommands are sent via RS-232 protoeol to individually eontrol eaeh set 
of Whegs™. The Sabertooth2xl2 has the eapability to eontrol up to eight different ehannels 
simultaneously using paeketized serial mode. Einally, the motor driver ean operate in a ramping 
mode that provides smooth aeeeleration upon reeeipt of eommands [21]. 
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Figure 3.18: Picture of the Sabertooth 2x12 motor controiier 



Figure 3.19: Picture of the Beikin Wireiess Adapter 


3.3.4 Network Card 

Wireless eommunieations is provided by a Belkin N Wireless Adapter (F5D8053), Figure 3.19. 
The USB deviee has data rates of up to 300Mbps under USB2.0 interface. New drivers were 
necessary to get the device to work correctly under Ubuntu 10.04. 


3.3.5 CMUcam3 

The CMUcam3, Figure 3.20, is a (352x288) RGB color camera designed for open source devel¬ 
opment for a variety of applications. It is capable of performance up to 26 fps, and can process 
images onboard prior to downloading to another computer. The CMUcam3 will be used for 
future implementation of robotic stereovision for obstacle avoidance. 
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Figure 3.20: Picture of the CMU camera 


MONTe 

Power Bus Architecture 



—Control System * ^ 'Drive System* 

Figure 3.21: MONTe Power Bus Architecture 


3.4 Power Bus 

MONTe’s power bus was designed to power both the eontrol hardware (proeessor, sensors), and 
the drive hardware. Furthermore, the power bus provides the appropriate voltage regulation and 
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protection necessary for operation of MONTe. The overall architecture is detailed in Figure 
3.21. The power for the bus is provided by stacks of Lithium Polymer (LiPo) batteries. Each 
stack provides 14.4V to the bus. A single stack powers the Control System, while two stacks 
wired in parallel power the Drive System. 

The Control System consists of the LPC-lOO computer, and the Monkey navigational unit. The 
LPC-lOO is fed off a RCB-DCDC006 12V Regulator. The Monkey is fed by a 0J2334 Pololu 
5V Regulator. Future sensors and control hardware will be incorporated on this bus. 

The Drive System consists of the Sabertooth2xl2 (unregulated), and the HS-7955 TG Tail Servo 
Motors. The servo motors are powered by three parallel Pololu 5V Regulators. This provides 
the necessary current to operate the tail in self-righting mode. The two LiPo battery stacks are 
in parallel to provide extended operating time. 

3.5 Communication Paths 

Transferring data is a vital component of any autonomous or unmanned system. MONTe uses a 
variety of paths for external and internal communications detailed in Figure 3.22. Both current 
implementation (solid lines), and future communications paths (dashed lines) are illustrated. 
The FPC-lOO serves as the central hub for both the internal communication paths (hardware), 
and for external communications with the base station. 

3.5.1 Internal Paths 

The internal communication paths for MONTe are to transfer information between the different 
sensors and processors. The two main formats utilized are RS-232 serial communications and 
User Datagram Protocol (UDP) communications. 

UDP is an Internet Protocol that operates in datagram mode. This provides a quick way of 
sending packets of information both over hard connection (cross-over cable) or via wireless. 

Motor Control 

The FPC-lOO communicates with the Sabertooth2xl2 motor drivers via RS-232 over COMl 
port. The motor drivers are operated in simplified serial mode and receive command bytes for 
each individual motor. The RS-232 signal must be stepped down to 0-5V TTF signal via an 
optical isolation circuit. The implementation is accomplished via a custom C-t-i- library based 
on code from Reference [22]. 
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Monkey 

The transfer of navigation data and flags is handled by a serial-USB interfaee. This allows better 
alloeation of serial eommunieation ports on the LPC-100. GPS data is sent in NMEA format. 
Compass data and eontrol flags are also sent over the path. 


CMUcam3 

The stereovision system will be integrated in the future via RS-232 on COM2. One eamera 
will be the master and be eonnected via its UART to COM2. The slaved CMUeamS will be 
eonneeted via the seeond general purpose input/output port (GPIO) on the master CMUeamS. 


Microcontroller 

MONTe has the funetionality to eonneet with a mieroeontroller via UDP using a eross-over 
eable. This provides a quiek way of ineorporating a mieroeontroller for integrating additional 
sensors. The eommunieation protoeol is designed to work with a Rabbit BL4S200 mieropro- 
eessor. 
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VNCserver 

MONTe runs a VNCserver in the background so the operator can make adjustments while ac¬ 
tive. Changes to the program and node architecture can be performed depending on needs of 
testing or the mission. 

3.5.2 External Paths 

External communications is how MONTe will interact with the operator during testing and 
missions. MONTe utilizes UDP IP communications with the base station through the wireless 
USB adapter. UDP is simple to implement and requires less processor overhead from the LPC- 
100. This will allow MONTe to receive commands, and transmit sensor data to and from the 
base station. 
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CHAPTER 4: 
Results 


4.1 Component Interfaces 

4.1.1 Motor Driver 

The Sabertooth2xl2 dual 12A motor driver was tested in both simplified and paeketized serial 
modes. The Sabertooth2xl2 ramping funetion was also tested to show a smooth aeeeleration 
of the motors. Simplified serial mode was implemented in the design due to its simplieity and 
effeetiveness. However, simplified serial was found to be ineompatible with the ramping feature 
of the Sabertooth2xl2. Simplified serial requires individual eommands for eaeh set of motors. 
It was found that sending the eommand for the seeond motor would override the first eommand 
if it had not reaehed the ordered speed. 

MONTe is eurrently working in simplified serial with no ramping funetion enabled. MONTe 
ean effeetively move using this mode. The laek of ramping is of eoneem due to wear on the 
drive train. However, this is aeeeptable for the eurrent iteration. 

Paeketized serial would solve this issue, but operation in that mode was not eonsistent. The first 
eouple of eommands to the plant would work as expeeted. However, errors would ereep in to 
the motor driver as the values ehanged yielding nondeterministie behavior. Paeketized serial 
will be neeessary for implementation of the water jets, so further researeh and development is 
neeessary. 


4.1.2 Navigation Data 

The interfaee between the CHIMU AHRS and the main program on the LPC-lOO is done via 
RS-232 protoeol at 115,200 Baud. Navigation data was sueeessfully reeeived using buffered 
serial at a rate of 4 Hz. The main data transferred ineludes: the number of satellites traeked, fix 
quality, three-dimensional veloeity, altitude, and heading. Latitude and longitude are in deeimal 
degrees to four plaees. 4Hz was seleeted to synehronize with the rest of the ROS program 
arehiteeture to provide a eonstant update of heading information for the navigational eontrol 
and eould be further optimized in the future. 


33 




Comparison of Compartment Temperature for Bench TestOI 
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Figure 4.1: Comparison of compartment temperature profile characterizations 


Bench Test #2; Temperature Rise vs Time 
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Figure 4.2: Compartment temperature change over time. 


4.2 Compartment Temperature Profile 

Because the LPC-lOO computer has a recommended maximum operating temperature of 40°C, 
two bench tests were performed to determine a reasonable high-end limit for continuous op¬ 
eration while the compartment was sealed. The LPC-lOO was started inside the chassis and 
employed a continuous counting program to stress the processor. The compartment tempera¬ 
ture was monitored during operation after the compartment was sealed. The first test was done 
with the LPC-lOO running off of external power, while the second was run off a 14.4V Lithium 
Polymer battery stack in the design configuration. 
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Figures 4.1 and 4.2 shows the eompartment temperature profile generated by both beneh tests. 
The initial internal eompartment temperature for both tests was 22°C. The first test took 32 
minutes to reaeh the eritieal temperature, while the second bench test took only 28 minutes 
on battery power. The inclusion of internal power dropped the time by 5 minutes and proved 
that the temperature profile is a concern. Future work will include heats sinks to manage the 
compartment temperature. 


4.3 Mobility 

4.3.1 Self-Righting 

Modeling Environment 

One of the major advancements of MONTe is the incorporation of an autonomous tail. To 
anticipate the behavior of the robot and improve the design process, MONTe was modeled 
using Working Model 2D (WM2D). This simulation environment models Newtonian equations 
of motion for interacting bodies and displays the output in an intuitive user interface [23]. It 
allows for interactive simulations that can receive input from user controls, scripts, and other 
applications, such as Excel and MATLAB. One drawback is that the software only models in 
two dimensions and therefore does not allow for three dimensional terrain and assumes stability 
in the roll direction. MONTe was modeled using approximate geometries and weights. This 
modeling environment provides a proving ground for various designs and control algorithms 
without requiring a test platform. 



Figure 4.3: WM2D Model of MONTe rotating its tail from the neutral position to the stow position while upside down 

Self-righting creates the most limiting condition for the required torque of the servo drive mech¬ 
anism. MONTe was modeled early in the design phase to estimate the necessary gear ratio for 
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Figure 4.4: WM2D Model of MONTe rotating its tail from the stow position to the neutral position while upside down 

the servo. There are two different scenarios in which the servo drive mechanism would be 
subjected to a large load. Figure 4.3 shows a model of MONTe righting itself from an initial 
neutral tail position. As the tail is retracting to the stow position, the servo drive mechanism is 
exerting a maximum torque of approximately 60 lb-in. This corresponds to 960 oz-in. Figure 
4.4 shows a similar model except the tail is initially stowed. This is the most limiting situation 
and requires the highest torque from the servo drive mechanism. The maximum magnitude of 
torque required is approximately 85 lb-in, which corresponds to 1400 oz-in. This value closely 
agreed with the torque expected from our design criteria and a simple lever arm from Section 
3.1.6. 

The two gears selected for use in MONTe have torque ratings corresponding to 1250 oz-in or 
2150 oz-in, based on supply voltage of 4.8 V [18]. Selection of a specific servo drive mech¬ 
anism, with either a 5:1 or 8.6:1 gear ratio, places a constraint on the servo drive speed. The 
rotation speed will either be approximately 45 or 75 °/sec based on the same supply voltage. 
This rotation speed will dictate the time it takes for MONTe to right itself, but will also impact 
the time response of the tail when climbing obstacles. Since the model shows that MONTe 
requires at least 1400 oz-in, the 8.6:1 gear ratio will be necessary to right MONTe. 

Field Trials 

MONTe was subjected to tests to determine if self-righting would be possible. At the time of 
testing MONTe weighed 19 pounds, just below the anticipated weight of 20 pounds. Initial 
tests showed that MONTe intermittently succeeded to flip with a 5:1 gear system installed. The 
initial reason for failure was due to reaching the upper limit on current supplied to the servo drive 
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mechanism. Additional regulators were placed in parallel to provide sufficient current. Success 
with the 5:1 gear ratio improved, however it was still intermittent and required upgrading to the 
8.6:1 gears. Tests were then conducted with repeated success using the 8.6:1 gear ratio. Figure 
4.5 shows MONTe successfully righting itself with those gears installed. This scenario is from 
the stowed position and places the servo drive mechanism under the largest load. Success from 
this position indicates that MONTe will be able to right itself from any variation of the scenario. 
This also ensures that there is substantial torque available to lift the rear of MONTe for climbing 
assist. 



Figure 4.5: Clips of video showing MONTe righting itself from the limiting scenario 


4.3.2 Climbing Assist 

Modeling Environment 

MONTe was again modeled in order to develop an algorithm to control the tail for climbing 
assistance. As discussed in Section 2.1.2, high centering is the common failure for similar surf- 
zone designs. In order to analyze this high centering condition MONTe was modeled climbing a 
six inch step. Figure 4.6. This screen shot from WM2D includes control parameters and output 
data. Figure 4.7 highlights the pitch data of the main body during the stalled climb. The slope of 
the pitch graph corresponds to the rate of change of that pitch, which nearly drops to zero when 
MONTe becomes high centered. This phenomenon is used to invoke the tail for assistance. 

The control algorithm. Appendix A. 1 and A.2, was then developed in MATLAB based on the 
adverse pitch rate during high centering. This control method actuates the tail in the event 
MONTe develops a high average pitch rate (> 4°/sec) followed by an low instantaneous pitch 
rate (< l°/sec). MONTe was then modeled on the same six inch step, but now linked to the new 
control algorithm in MATLAB. Figure 4.8 shows MONTe successfully climbing the previous 
obstacle. This algorithm was then programmed onto the Monkey board for real obstacle testing. 
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Figure 4.6: Model of MONTe encountering a high center condition 
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Figure 4.7: Pitch data from a high center scenario 


Since the tail eontrol algorithm was written in MATLAB based on WM2D, some modifications 
were necessary to host the algorithm on the Monkey board. Migrating the eontrol program from 
MATLAB to the Monkey in C language required two major changes. First, the program would 
output desired tail angle vice tail speed. WM2D does not contain servo motors as deviees and 
only allows eontrol of rotational motors through torque, speed, or position values. When using 
the position method it would instantaneously move the tail to the new preseribed angle. In 
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Figure 4.8: MONTe, in WM2D, interfaced with MATLAB taii controi aigorithm to overcome an obstacle 

order to overcome this inaccuracy, tail speed was controlled vice angular position. For the real 
servo, the desired angle is prescribed through PWM and the inherent servo electronics control 
the actual speed to arrive at that angle. Therefore the output was revised to control tail position. 
Secondly, the real AHRS presents additional noise beyond that seen in the pitch rate of Figure 
4.6. Since this noisy signal of raw pitch rate had the potential for creating erroneous tail control, 
the pitch rate was instead calculated from the pitch angle. The sampling rate for the pitch angle 
was set and used to calculate an average pitch rate. This time average was used in lieu of raw 
pitch rate and successfully suppressed the erratic signal. The Monkey control algorithm can be 
found in Appendix B. 

Field Trials 

Field trials were conducted with the modified control algorithm. MONTe was successful at 
actuating the tail when the front was lifted to simulate a stall condition. Additional testing was 
conducted while driving MONTe over obstacles manually. For these tests the tail was con- 
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Performance Test #1: 
Velocity vs Command 



0 10 20 30 40 so 

Serial Command 

Figure 4.9: Performance curves over concrete 

trolled autonomous by the Monkey board. MONTe repeatedly failed to elimb step obstaeles 
after multiple attempts. During this testing phase, several design issues were noted. The sus¬ 
pension system was not stiff enough and eaused MONTe to travel low as it reaehed an obstaele. 
The Whegs™were also not equipped with any form of traetion and prevented MONTe from 
advaneing onto the obstaele onee the tail was lowered. Additional testing will be required to 
prove MONTe’s overall design and in order to tune the tail eontrol algorithm, see Seetion 5.1. 

4.3.3 Speed of Advance 

MONTe was tested over eonerete to develop the motor eontrol algorithms. Data was taken over 
a full range of eontrol signals for forward velocity. Results are detailed in Figure 4.9. 

Maximum velocity was determined to be 3.4 m/s for simplified serial command of 64. A nom¬ 
inal traveling velocity of 1.6 m/s was selected (serial command: 30). The maximum turning 
differential (without slipping) was 10. 

4.4 ROS Testing 

ROS testing explored the capabilities of the operating system in manual control and autonomous 
navigation. MONTe’s program was successfully able to operate in full capacity. Figure 4.10 
shows the current architecture using the rxgraph utility provided by ROS. Each active node 
is displayed with the connecting topics between nodes illustrated as well. 

Each connection represents a subscribe, or publish path through the labeled topic. This pro- 
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Figure 4.10: Output of ROS rxgraph function showing MONTe’s node architecture 

vides an easy display of the dependeneies of eaeh node. The Waypoint_Control, and Key¬ 
board-Control were both sueeessful in transmitting user eommands to the rest of the system. 

4.4.1 Remote Control 

Remote operation of MONTe used a VNCserver for remote aeeess during operation. The tests 
were sueeessful as different program nodes eould be started or stopped over a wireless network. 

For example, MONTe eould be run in manual eontrol only by starting ROSeore, Keyboard 
Control, and Plant Control nodes. This also allowed ehanging and reeompiling ROS nodes 
during testing. This proved useful during ealibration of plant eontrol eonstants. 

MONTe eould be remotely piloted as well. By running the nodes Keyboard-Control and Plant-Control, 
MONTe eould be given eommands to go forward, reverse, turn left, and turn right. The forward 
and reverse veloeities, and turning rates eould be adjusted. 

Unaeeeptable lateney was eneountered during remote operation. Lag times of upwards of 15- 
30 seeonds were observed in proeessing eommands and reeeiving program responses while on 
VNC. MONTe would be unresponsive to eommands during these periods of lag. This was 
not an issue during autonomous navigation, but did pose a signifieant issue when in manual 
eontrol. Lateney was not unexpeeted, though, and will drive future efforts to perform most 
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control via Java-based GUI. Relegating the VNC to providing means of deep modifieations of 
the operational program will relieve the proeessor and optimize performanee. 

A seeure shell protoeol (SSH) is another option for remote operation. This only brings up a 
Unix shell for interaeting with the system, so has less overhead assoeiated with it. Multiple 
SSH eonneetions eould be made to bridge the gap until a more sophistieated interfaee ean be 
implemented. 
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CHAPTER 5: 
Future Work 


5.1 Improvements to an Autonomous Tail 

5.1.1 Tail Design 

While modeling MONTe and conducting field tests, several design aspects were considered 
problematic. In one scenario the cross member for the tail protrudes too far below the chassis 
height. This limited ground clearance caused a unanticipated stall condition and required cy¬ 
cling the tail in order to free the tail. The cross member was originally included in the design in 
order to add rigidity to the tail and prevent it from twisting. Further testing should be conducted 
on the tail to either relocate or remove the cross member from the design. Additionally it may 
be possible to remove any support between the two sides of the tail and instead drive them as 
individual units. This would no longer require tuning the servo drive mechanisms to ensure 
that they are operated in tandem. This would also allow the tail to be controlled in a manner to 
induce a roll change as well as a pitch change. 

Some testing scenarios also demonstrated that the friction at the end of the tail sometimes lim¬ 
ited the forward motion of the robot. If the tail was invoked to assist with climbing on soft 
terrain, there was a chance that the tail would become almost embedded in the ground as it 
acted to lift the rear of the robot. The limited surface area of the tail tip and the high friction 
could cause a stall. It may be possible to reduce the susceptibility to this condition by adding a 
component to the tail that would instead roll along the ground. This concept would reduce the 
friction at the tail end and allow the traction from the Wheg™ to pull the robot in the desired di¬ 
rection of travel. Future testing should investigate the multitude of these design improvements. 

5.1.2 Tail Control Algorithm 

Currently the tail control program only uses the pitch data from the Monkey’s CHIMU module 
to detect a high center condition and to actuate the tail. Additional sensors can be used to 
more accurately anticipate and detect a high centering condition. One would be to measure 
the force exerted on a Wheg™. When that force goes to zero, the Wheg™ can be considered 
unloaded. This can be easily sensed by limit switches attached to the suspension inside the drive 
assemblies. Also, the CHIMU’s accelerometer data can be used to determine if forward motion 
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has ceased, indieating a stall eondition. This would require interfaeing with plant eontrol to 
ensure that the stall is not intended. 


5.2 Improvements to Program Architecture 

The eurrent arehiteeture will serve as the framework for future development of MONTe. Many 
ROS eoneepts have been proven and implemented sueh as the publisher/subseriber method of 
intermodal eommunieations. The sueeessful integration of sensors and hardware sueh as the 
Monkey, and the Sabertooth2xl2 shows the usefulness of ROS for developing robotie projeets. 

Numerous features ean be improved or added to in the existing arehiteeture. The eurrent eode 
was developed using eoneepts from both C and C++. The overall arehiteeture is objeet-oriented. 
Nodes are essentially “objeets” that perform task, and eommunieate with others. Some of the 
legaey eode utilized in development from previous projeets was written in C, and is therefore 
not objeet-oriented. A traditional, linear based programming ean easily be at the merey of bugs 
that appear in the system, and its nature deereases its “portability”. Portability allows eode to 
be used in a variety of uses, and is one of the main goals of ROS. Therefore, the eode needs to 
be modified in aeeordanee with the prineiples of objeet oriented programming. 

The goal would be then to modify, and eonduet partial rewrites of the individual nodes them¬ 
selves. This would bring the eode in line with the prineiple of objeet oriented programming. 

5.3 Navigation, Mapping, and Object Avoidance 

Future work in navigation, mapping and objeet avoidanee will inelude: 

1. Implement additional sensors to improve navigation and autonomy. 

2. Test navigational algorithms to optimize performanee. 

3. Utilize stereovision for objeet loealization and avoidanee. 

Proposed sensors would inelude laser range-finders, aeoustie sensors, and stereovision. 

Previous work by Baravik [24] developed algorithms to eonduet edge-deteetion and subsequent 
ranging. His proeess started by identifying the eontours in the seene. These eontours are then 
eorrelated to piek out the objeets. The range is then determined by getting an angle to pixel of 
highest eorrelation by finding the pixel separation. The eode was not implemented for real-time 
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detection due to limitation of the camera and microprocessor. MONTe, however, has greater 
computational capacity in the LPC-100. Research could be done to develop a ROS node that 
could image the environment and process the results for object detection[24]. 

Path planning goes hand in hand with object detection. A simple algorithm is the “bug” al¬ 
gorithm. MONTe determines the heading to the current destination and drives the path until 
discovery of an obstruction. MONTe could then follow the perimeter of the obstacle until clear. 
The robot would then resume its heading to the destination [14]. 

A more sophisticated option would be to actively map the operating environment. MONTe 
would input navigational and sensor data to create a potential map that assigns strengths to the 
terrain it encounters. An algorithm could then be developed to determine the optimal path using 
Lagrangian or energy conservation techniques. This method could utilize MONTe’s terrain 
capabilities to drive over rough terrain if necessary as opposed to avoiding all terrain [14]. 

5.4 Remote Operation 

One of the primary issues currently confronting the design is the rudimentary user interface. 
While effective for initial testing of the operational program, more sophisticated testing will 
require a more sophisticated interface. Such a program would need to be able to load way- 
point routes, read sensor data, allow for remote control of MONTe, and a host of other useful 
functions. 

The legacy NPS GUI displays: 1) positional data, 2) a location chart, 3) manual control panel, 
4) waypoint routes. Additional features should include system status metrics for the power bus, 
environment, and other data of interest. Graphical displays could be expanded to include using 
of Google Earth™ to make the program suitable for general use. A display function for the 
potential map (filtered and unfiltered) would be useful to monitor the effectiveness of MONTe’s 
mapping process. 
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APPENDIX A: 

Simulation Tail Control Code 


A.l MATLAB Control Algorithm 


This controls MONTe’s tail and is used in conjunction with Working Model 2D and MATLAB. 


% - 

% MONTe TAIL CONTROL 
% Version 2.0 
% Written by Steven Halle 

% Thesis: Design and Implementation of a Semi—Autonomous Surf—Zone Robot 
% using Advanced Sensors and a Common Robot Operating System 
% 30MAY201I 


% Details for interfacing WM2D with MATLAB are found in WM2D user guide 


function tail_spd = TAIL2(body_rate ,body_angle ,tail_angle) 
% values inputted from WM2D 
% (pitch rate, pitch angle, tail angle) 

% outputs the tail motor speed 


% declares variables as global to hold values after the function is called 
global ab % shorthand for angle of body rate (pitch rate) 

global climb % 1 or 0 to indicate tail assitance needed 

global avg.rate % holds the value for the averge pitch rate 


k=size ( ab , 2 ); 
for n = l:k-l 

ab ( 1 , k—n + 1 )= ab ( 1 , k—n ); 

end 

ab(l,l)= body_rate; 
avg_rate =mean( ab , 2); 


loop to keep most recent pitch rate in ab 


logs current body rate as first in history 
takes average of entire ab array 


ab_new=ab(l ,1:6); 

new .rate =sum( ab.new ). / 6 ; % local variable , average newest 6 pitch rates 


% CONTROL LOGIC 

if(avg_rate > 4) && (new.rate < 1) 
climb = 1; 
t ai 1 _ s p d =60; 

elseif (body.angle > 5) && (climb==l) 
t ai 1 _ s p d =60; 

elseif ( body.angle < 5.0) && (tail.angl 
t ai 1 _ s p d = —60; 
climb =0; 

else 

t ai 1 _ s p d =0; 


% presents a stall climb situation 
% sets climb mode 
% actuautes the tail 
% continues tail until robot levels 

> 145.0) % stows the tail 
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end 


A.2 MATLAB Initialization 

This initializes MATLAB in order exeeute Appendix A.l. 

% - 

% MONTe TAIL CONTROL INITIALIZATION 
% Version 2.0 
% Written by Steven Halle 

% Thesis: Design and Implementation of a Semi—Autonomous Surf—Zone Robot 
% using Advanced Sensors and a Common Robot Operating System 
% 30MAY2011 

% - 

% This code is to be used in conjunction with Working Model 2D and MATLAB 
% Details for interfacing WM2D with MATLAB are found in WM2D user guide 

% Enables MATLAB proxy to interface with external application 
enableservice ( ’ AutomationServer ’ , true ); 

% Initializes global vairables used in control program and sets value to 0 

global climb avg_rate ab 

ab=zeros(1,100); 

avg.rate =0; 

climb =0; 
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APPENDIX B: 

MONTe Tail Control Code for Monkey Board 


This code is an edited portion of a Ryanmeehatronies suite that the Monkey Board hosts for 
autonomous tail control of MONTe. 

MODULE: UserMode. c 

VERSION: 1.00 

CONTAINS: Specific commands and modes for user actions 

COPYRIGHT: Ryan Mechatronics 
Date: Dec. 2009 

INSTALLED CODE FOR AUTONOMOUS TAIL CONTROL 
Edited by Steven Halle 

Thesis: Design and Implementation of a Semi—Autonomous Surf—Zone Robot 

using Advanced Sensors and a Common Robot Operating System 
Date: 30MAY20II 

NOTES: This code is provided as part of suite from Ryan Mechantronics. 

Portions of the supplied code were edited to create a tail control algorithm. 

The majority of code pertaining to MONTe resides in this file. Additional 
changes can be found in the errata section below 

Additional Change Section 

pwm.c C :\.. .\Common\Public\PWM 3 occurrences 
Line 38 //Edit by Steve Halle — 

only servos 5/6 will be used for MONTe 
Line 61 //Edit by Steve Halle — 

Sets configuration for Servos 5/6 according to MONTe design 
Line 118 //Edit by Steve Halle — 

ensures that servo does not get passed value outside the range of servo 

pwm.uplink.c C :\.. .\Common\Public\PWM 5 occurrences 

Line 15 static int bRC.Uplink.Active = FALSE; //Edit by Steve Halle — 
unsure if needed to invoke uplink 
Line 49 //Edit by Steve Halle — 

changed the function call in order to include input channel parameter 
Line 52 //Edit by Steve Halle — 

removed previous argument type, must now pass SERVOJN_1 , etc 
Line 100 //Edit by Steve Halle — 

removed previous argument type, must now pass SERVOJN_1 , etc 
Line 107 //Edit by Steve Halle — 

removed previous argument type, must now pass SERVO-IN^I , etc 
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control. c C:\.. . \ Platforms\Public\Generic 7 occurrences 
Entire file Removed from Project //Edit by Steve Halle — 
removed since PID not necessary for servo control 

projectconfig .h Monkey_User.Sandbox_SWD 1 occurrence 

Line 137 #define CFG.CHIMU.ORIG //Edit by Steve Halle - 

use autoselect is unknown, else orig as of 4/2011 

#include ’’globals.h” 

#include ” uart . h” 

# i n c I u d e ” IpcUART . h ” 

# include ’’math . h” 

#include ” iap . h” 

#include ’’string.h” 

#include ’’util.h” 

#include ’’system.h” 

#include ’’main.h” 

#include ’’CommOutput. h” 

#include ’’UserMode. h” 

#include ” events.h” 

#include ” adc . h” 

#include ” spi . h” 

#include ”gps . h” 

#include ’’stdio.h” 

#include ’’navigation .h” 

#include ’’guidance.h” 

#include ’’control.h” 

#include ”sd_logger.h” 

#include ’’pwm.h” 

#include ”pwm_uplink . h” 

#include ” ..\..\ Private\User_Library_Functions\UserFunctions .h” 

//If include Common\ Public\FFT\ dsplib.testbench _fft .main . h 

#ifdef CFGJNSFILTER 

#include ”ins_fi1ter.h” 

#endif 

#ifdef CFG_BMP085 
#include ”bmp085.h” 

#endif 


unsigned char user_mode = 0; //State machine status 
// Nav and guidance variables 
NAVIGATE nav-sol ; 

GUIDANCE guide_sol ; 


//Edit by Steve Halle — 

// need add pwm.h since control () removed 

// Edit by Steve Halle — 

// allows pass thru FWM signals 
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//Edit by Steve Halle 

//Global Variables declared outside user function loops 


float 

body.angle ; 

// This 

hold 

value for 

the 

angle of 

the pelican case of 

the 

robot 

float 

b.a.prev ; 

//Body 

Angle 

previous 






float 

body .rate ; 

// This 

holds 

value for 

the 

rate at 

which the body .angle 

is 

changing 

float 

avg.body.rate ; 

// This 

holds 

value for 

the 

average 

rate of body change 



float 

b.r.history [40]; 

// This 

holds 

a history 

of 

the last 

40 body .rate 








// {2 seconds worth if executed 

every 50ms ) 

float 

tail.output ; 

// This 

holds 

value for 

the 

desired 

tail position 



int 

climb ; 

// This 

holds 

status fo 

r the need fo 

r tail to take climbing 

action 


void User_Init( void) 

{ 

Led_Off(LED_RED); 

//Edit by Steve Halle — below code is copied from control. c to eliminate autopilot 
// - 

int i ; 

//INIT FWM base 

//Change this to PWMJOMSECSASE for 100 Hz updates to servos 
PWM.Init (PWM^OMSEC_BASE); 

//Initialize outputs 
Servo.Config (); 

//The below may be moved. 

//Servo values should be updated to real initial values before PWM is started 
//to make sure no startup glitch occurs 

//Start servos. Future Servo ^Set calls will just update value for a particular channel 
//or a general update on all of them 

PWM_Start(); 


//- end EDIT by Steve Halle - 

} 

void User.Main ( void ) 

{ 

//Main loop 

// Note that this is the time / tasking loop. 

//There are protected areas in this section you should leave alone. 

// State machine for task processing by user is in user .process () 

// 

//INIT Guidance , Nav and Control Modules ( including PWM’s) 

//NOTE: Waypoint has been commented out until we get lAP going on Cortex 

Waypoint_Init (); 

Navigate_Init (); 

//Edit by Steve Halle — removed control-init() and Guidance .Init () since autopilot not used 


55 



// - 

//Guidance.Init(&guide.sol) ; 

// Control.Init () ; 

// - end EDIT - 

#if defined CFG_SDCARD 

bLoggerOK = sd_init(); //Init SD card logger if enabled 
#endif 

gThruputCnts = 0; 

User.Init (); 


//Start processing loop 
while (1) 

{ 

gThruputCnts++; 

//Pseudo —tasking occurs here 
if ( time_lms_flag ) // 1ms (1000 Hz) tasks 
{ 

/* - */ 

/* BEGIN PROTECTED — DO NOT REMOVE — UNIT WILL NOT OPERATE CORRECTLY */ 

//Handle serial port receipt 
Main.SerialPort.Process (); 

//Handles double buffering of input from serial corns 

//Parse now that the interrupts seem over (RX EIEO has been emptied) 
SSPl_Parse_CHIMU(); 

//Parse GPS 
GPS_Parse (); 

//Process uplinked waypoints — Event handler will indicate if there 
//is a complete set waiting for you 
Waypoint-Process (); 

//INS filter processing 
#ifdef CFG-INSFILTER 
INS_Filter_Process (); 

#endif 

#ifdef CFG_BMP085 

B aro-Proces s (1000) ; //Updates pressure sensor at a 1 second rate. 

//Maximum update rate is about 40 msec 

#endif 

/* END PROTECTED -*/ 

/* - */ 

User_Process (); //Handles user processing (like mode switches , etc) 

time_lms_flag = // Clear time tick 

} 

if ( time_5ms_flag ) //5ms (200 Hz) tasks 
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//No Tasks 

time_5ms_flag = 0\ // Clear time tick 


f ( time _ 1 0ms.flag ) //10ms (100 Hz) tasks 
//No Tasks 

time.lOms.flag = // Clear time tick 


f ( time_20ms_flag ) //20 ms (50Hz) tasks 

/* - */ 

/* BEGIN PROTECTED — TX) NOT REMOVE — UNIT WILL NOT OPERATE CORRECTLY */ 

// Control-Process(& guide-Sol}; 

//Handle control functions (servos) at 50Hz 
//Edit by Steve Halle 

//removed as a test to eliminate 

//control functions for built in autopilot 

# i f d e f CFG.USEJVIONKEY.TELEMETRY 
TX.Com.Process (); //Handles output messages 
#endif 

/* END PROTECTED - 

/=*=- 

time_20ms_flag = // Clear time tick 


*/ 


f (time.lOOms.flag) //100ms (10 Hz) tasks 

/*-*/ 

/* BEGIN PROTECTED — TX) NOT REMOVE — UNIT WILL NOT OPERATE CORRECTLY */ 

ADC-Process (); // 10 Hz, start a burst ADC read. Global gADC holds result. 
gOK.TO.SEND = TRUE; //DEBUG — Spits CHIMU returning messages back 

/* END PROTECTED -*/ 

/* - */ 

//Below is for special output on UART 1 
// User_NMEA_Output (); 

//Below is for SD card logging of standard data set if card is present 
//Called at 10 Hz, but only writes to disk after 10 entries 
#if defined CEG_SDCARD 
SD_StandardLogging (TRUE); 

#endif 


time.lOOms.flag = 0; //Clear time tick 


57 



} 


if (time.lOOOms.flag) //1000ms (1 Hz) tasks 

{ 

System.Check.CPU (); //Checks CPU load and puts it into Monkey output message as needed 
// Waypoint_demo (); 

//Edit by Steve Halle — passes GPS posit from spare port in NMEA output every 1 sec 
User_NMEA_Output (); 

time.lOOOms.flag = 0; //Clear time tick 

} 

}//Loop back around 

} 


void User.Process ( void ) 

{ 

//This is where mode specific actions should happen. 
// It is where most of your decision making occurs 

static unsigned long lasttime = 0; 
static unsigned long elapsed.time = 0; 
unsigned long dt.msec = 0; 

dt.msec = getTimeCounts () — lasttime; 
lasttime = getTimeCounts (); 
elapsed.time += dt.msec; 


User.Event.Process (); //Go check for events that may have occurred (user event messages) 

switch (user.mode) 

{ 

case(0): 

// First two cases are for testing and debugging purposes 

//Example mode 0 

if ( elapsed.time > 1000) 

{ 

uartOPuts (’’Here I am\r”); 

Led.Flash (LED_RED, 1); 
user.mode ++; 
elapsed.time = 0; 

} 

break; 
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case(1): 


//Example mode 1 

if ( elapsed-time > 1000) 

{ 

Led_Flash(LED_RED,2); 
user.mode ++; 
elapsed.time = 0; 

//servo _out [SERVO-RIGHT_6 ]. value = 110; //for debugging servo output 

} 


break; 
case(2): 

//Example mode 2 


if ( elapsed.time > 200) 

{ 


//loop executed every 200 
/ / IMPORTANT: calculations 


msec 

below use this time step 


int i ; 

float tot_b_r=0; 

for (i = 40; i >0; i —) 

{ 

b.r.history [i—l]=b_r_history [i —2]; 

//performs a shift in the history array 
//— b _r .history [0] remains value zero 

} 

for (i = 1; i <40; i ++) 

{ 

tot _b_r = to t _b _r + b.r.history [ i ]; 

//calculates the sum of all the 
//b.r.history elements 

} 

avg.body.rate = tot.b.r /3 9; 

//calculates the average body rate based on the 
//last 39 array elements ( b .r .history [0] = 0) 

b_a_prev = body_angle ; 

//assigns the previous body.angle 
body.angle = gAttitude . euler . theta ; 

//sets body angle to current pitch angle of 
//Monkey (in units radians) 
b.r.history[0] = (body.angle — b.a.prev )/0.20; 

//calculates rate based on 50ms calc steps 


if (servo.in [0] .ro.msec > 1.5) 

//servo .in [ 11 matches SERVO-INPUT.2 

{ 

Led_On(LED_RED); 

//RED LED on Monkey means board is in manual control 

PWM_Uplink_Passthru( SERVO_INPUT_2 , SERVO .RIGHT .6 , FALSE ); 
//input channel, output, failsafe — see prototype 

PWM_Uplink_Passthru( SERVO_INPUT_3 , SERVO_LEFT_5 , FALSE ); 
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//input channel, output, failsafe — see prototype 

} 

else 

{ 

//Autonomous Mode 
// TAIL LOGIC 
Led_Off(LED_RED); 

if (( avg_body_rate > 0.045) && ( b _r _h i s t o r y [0] < 0.035)) 

{ 

//sense high center condition 
climb = 1; 

servo.out [SERVO_RIGHT_6]. value = 225; 
servo_out [SERVO_LEET_5 ]. value = 225; 

Servos_Update_All (); 

} 

else if (( body_angle > 0.09) && ( climb == 1)) 

{ 

//continue to hold tail until climb complete 
servo_out [SERVO_RIGHT_6]. value = 225; 
servo.out [SERVO-LEET_5 ]. value = 225; 

Servos_Update_All (); 

} 

else //( body .angle < 0.09) 

{ 

//return to neutral position 
climb =0; 

servo.out [SERVO_RIGHT_6]. value = 115; 
servo.out [SERVO.LEET.5 ]. value = 115; 

Servos.Update.All (); 

} 

} 


} 


break; 

// Edit by Steve Halle 


e 1 a p s e d _ t i m e 


0 ; 


} 

} 


int User.Event.Process ( void ) 

{ 

User_Uplink_Msg cmdmsg; 

switch ( Event_Retrieve ( &gEvent)) 

{ 

case —1; 

return ( — 1); 
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break; 
case 1: 

//Event has been loaded into global, now figure out what to do with it 
if (gEvent.id != EVT_UPLINKJVISG) return (-1); 

//This ignores other uplink messages , 

//like waypoints , etc... that are handled by the main, common processing 
memmove( &cmdmsg , &gEvent. payload , gEvent. length ); 

// If a mode switch , take it and leave 
if (user.mode != cmdmsg . mode) 

{ 

user.mode = cmdmsg . mode ; 
return (1); 
break; 

} 

// 

//For example message, we have a command word that 
//indicates what command has been sent: 

// 

switch (cmdmsg . action ) 

{ 

case USER_ACTION J^EQUEST_STATUS: 

User.Status.Output (); 

break; 

case USERj^CTION.ABORT: 
break; 

case USERJ^CTION.CMD JUNGLES: 
guide_so 1 . a11 _des . euler . phi = 
guide.sol . att.des . euler . theta 
gu ide _s o 1 . a 11 _de s . eu ler . p s i = 

break; 

return (1); 
break; 
default : 

return (— 1); 
break; 

} 

} 

} 


unsigned char User .Status .Output ( void ) 

{ 

// This is a message wrapper for special user messages. 
//Normal telemetry contains most items of interest 
// This message wraps output in the user message format OxAA 
// 

int index = 0; 

short int sint.tmp = 0; 

float ftmp = 0.0; 


cmdmsg. phi_desired ; 
cmdmsg. theta_desired ; 
cmdmsg. psi_desired ; 
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int i = 0; 

if ( gUserBytesOut . isbusy ~ TRUE) return (FALSE); 

//Buffer hasn’t been transmitted yet, don’t respond yet 
// Example output message is: 

// Modes / status 
// Timer / counters 
// Example float data 

// 

//Modes 

gUserBytesOut. payload [ index ]= 0x01; index++; 

//User message ID — chose 0x01 for no good reason 
gUserBytesOut. payload [ index]= 0x00; index++; 

//User message length (overwrite at bottom once index is summed up) 
gUserBytesOut. payload [ index ]= user_mode ; index++; 

//Local state machine 
// Timers / counters 

memmove(&gUserBytesOut. payload [ index ] , 

&gTime_Msec , sizeof(unsigned long)); 

index += sizeof (unsigned long); 

//System running time in milliseconds 
memmove(&gUserBytesOut. payload [ index ] , 

&gThruputHz , sizeof(unsigned long)); 

index += sizeof (unsigned long); 

// Thruput 

// Example float data 
ftmp = (99.0); 

memmove(&gUserBytesOut. payload [ index ] , &ftmp , 
sizeof ( float )); 

index += sizeof ( float ); 

// Done now with populating 

// Now, replace the length byte with our total index to help decoding 
gUserBytesOut. payload [ 1 ] = index ; //No bumping index here, we are just 
gUserBytesOut. length = index; 

// Now, the global user bytes are all setup and ready to go 
// Flag it as full , then send it all out using the message OxAA user 
// When it is gone, the flag will be cleared 
gUserBytesOut. isbusy = TRUE; 

Tx.Corn_Add_Special.Message (MSGOut.OxBO.User.l ); 
return (TRUE); 


unsigned char User.NMEA.Output ( void ) 

{ 

//Example output of various data in NMEA format on the spare UART 

//For details of GPS structure see globals.h 

int index = 0; 

int i = 0; 

char stemp [12 8]; 


by a ground station 
replacing a value 

bytes wrapper 
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char pOutString [ 1 96]; 
unsigned char pFieldXSUM = 0; 


// Send Header first 
sprintf ( pOutString , ”$RM, ” ); 

// Add other data 
//GPS first 

sprintf (stemp , ”%01 i ,%08 Id ,%08 Id ,%03 Id ,%041d ,%04 Id ,%041d ,%1X , ” , (int)gGPS. satstracked , 

(long ) (gGPS . latitude *10000),(long ) (gGPS . longitude*10000), 
(long ) (gGPS . altitude ) ,(long)( gGPS . velN * 10), 

(long)(gGPS. velE*10), (long )(gGPS . velD * 10), (long ) (gGPS .TOW)) 

s t r c a t ( pOutString ,stemp); 

sprintf (stemp , ”%041d ,%041d ,%041d ,%041d ,%041d ,%041d ,%041d ,%041d ,%041d , 

(Iong)( gAttitude . euler . phi*100), 

(long)( gAttitude . euler . theta *100), 

(Iong)( gAttitude . euler . psi*100), 

(long)(gSensor. rate [0]*100), 

(long)(gSensor. rate [1]*100), 

(long)(gSensor. rate [2]*100), 

(Iong)(gSensor.acc[0]*100), 

(Iong)(gSensor.acc[l]*100), 

(long)(gSensor.acc[2]*100)); 
s t r c a t ( pOutString ,stemp); 

#ifdef CFGJNSFILTER 

sprintf ( stemp , ”%081d ,%08 Id ,%03 Id ” , 

(long ) ( gINS .latitude *10000), 

(long ) ( gINS . longitude*10000), 

(long ) ( gINS . altitude )); 
s t r c a t ( pOutString ,stemp); 

#endif 

//OK, now we have the whole string . Time to find the checksum 
for (i = l; i<strlen ( pOutString ); i++) 

//NOTICE: Starting at 1, because xsum doesn 't use $ in front 

{ 

pFieldXSUM "= pOutString [ i ]; 

} 

//Tack it on the end 
sprintf ( stemp , ”*%02x” , pFieldXSUM ); 
strcat ( pOutString ,stemp); 

//Output the string 
uartlPuts (pOutString ); 

// Output the <CRXLF> 
uartlPutch(0 xOD); 
uartlPutch(0 xOA); 

return (TRUE); 

} 



THIS PAGE INTENTIONALLY LEET BLANK 
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APPENDIX C: 
ROS Code 


All electronic versions of this code will be incorporated into the upcoming NPS ROS Stack. 

C.l Navigation Node 

Following code performs navigation for MONTe. 

Title: MONTe_Nav 0.0 (ROS Node) 

Author: Jason Hickle 

Purpose: Node that allows MONTe to navigate through the world. 

Handles the following functions : 


1) Monitors Command^Flags to determine behavior. 

2) Pulls current position data (GPS and heading) 

3) Gets current waypoint 

4) Computes desired heading and range to waypoint. 

5) Computes plant commands using current and desired hdg. 

6) Updates Command.Flags and Plant .Control 


Use: 


Communication protocol handled via ROS messaging. Launch program as 
part of roslaunch . 


Runs at loop rate of 4 Hz. 


ROS Notes: 


Name— 


” MONTe .Navigation ” 


Publications — ” Plant.Command.T” 

” Command.Flags.T” 


Subscriptions — ” Command.Flags.T” 


’’Nav.Data.T” 

” Current -Waypoint -T ” 


Messages — 


Command-Flags. msg 
Plant-Command. msg 
Waypoint. msg 


Services — 


None 


Version History: 


- Version 0.0 - 

Mar list , 2011 
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LT Jason Hickle 


Enable Manual Control mode in main control loop. 


/* 

#iiiclude 

#iiiclude 

#iiiclude 

#iiiclude 

#iiiclude 

#iiiclude 

#iiiclude 


Libraries 
<ro s/ro s.h> 

<math . h> 

”std_msgs/String .h” 
’’MONTe/Plant.Command . h 
"MONTe/ Waypoint. h” 
’’MONTe/ Command.Flags . h 
”MONTe/Nav_Data . h” 


*/ 


// Message format 
// Message format 
// Message format 
// Message format 


(.msg) 

(.msg) 

(.msg) 

(.msg) 


#iiiclude <sstream> 


/* 

D efi nes 

*/ 


const 

double PI 

= 3.14159265; 


const 

const 

unsigned 

unsigned 

char STOP_R = 190; 

char STOP_L = 64; 

// 

// 

const 

const 

unsigned 

unsigned 

char R_CORRECT = 1; 
char L.CORRECT = 3; 

// 

const 

const 

double RANGE_THRESH = 2.5; 

float HDG_ERROR_THRESH = 3.0; 



Right motor stop 
Left motor stop 

Correction factor to calibrate forward speed 


const char 
const char 


FWD SPF,F.D = 30; 
MAX_TURN_DIFF = 


10 ; 


/* 

/ 

/ 

typedef struct 


CF holds all command flags. Expand as necessary. Ensure publishing 
utilizes default values for any flag unchanged to ensure flags are 
not being changed une c e s s arily . 


/ 

/ 

*/ 


{ 


}CF; 


bool autonav; 
char mode; 
bool route; 


//Define flags structure 


/* WPJ^ stores all data necessary to navigate to, and determine 

/ behavior mode for a route. 

typedef struct 

{ 

double lat ; 
double Ion ; 
char action ; 


/ 

*/ 


}WP; 


//Define waypoint structure 


/* 


NAVDATA is for current positional data 


*/ 
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typedef struct 

{ 

double lat ; 
double Ion ; 
double heading ; 
}NAVDATA; 


//Define Current Position structure 


/* Global Variables */ 

WP Current.WP ; 

CF Cmd.Flags ; 

NAVDATA Current_Nav_Data ; 

double K_P_coefficient ; // Proportional constant for PID control 

float right_command , left_command ; 

/* Functional Prototypes */ 

void Command_FlagsCallback( const MONTe:: Command_FlagsConstPtr& flag s ); 
void WaypointCallback ( const MONTe: : WaypointConstPtr& way.pt); 
void Nav.DataCallback ( const MONTe: : Nav_DataConstPtr& nav.dat); 
int Navigate (); 

int Plant.Compensator (double range, float desired.hdg , float current.hdg ); 

int main(int argc , char **argv) 

{ 

/* ROS Initializations */ 

ros :: i n i t ( argc , argv , ”MONTe_Navigation” ); // Set up ROS node 

ros :: NodeHandle n; // set up handle for this node 

// Set up all publishers for node 

ros :: Publisher plt.cmd.pub = n . advertise <MONTe:: Plant-Command >(”Plant_Command_T” , 1) 
ros :: Publisher cmd_flg_pub = n . adverti se <MONTe:: Command-Flags >(”Command_Flags_T” , 1) 

// Set up all subscriptions for node 

ros :: Subscriber sub-cf = n . subscribe (”Command-FlagS-T” , 1, Command-FlagsCallback ); 
ros :: Subscriber sub-wp = n . s ub scribe (” Current-Waypoint-T ” , 1, WaypointCallback); 

ros :: Subscriber sub-nav = n . s ub scribe (” Nav-Data-T” , 1, Nav-DataCallback ); 

ros:: Rate loop-rate (4); // Sets 4hz cycle for main loop 

// Set up message handles for communicating with topics. 

MONTe:: Command-Flags flags , pub_flags ; 

MONTe:: Waypoint way_pt; 

MONTe: : Plant-Command cmd; 

MONTe: : Nav.Data nav.dat ; 
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int nav_val = 0; 

Cmd.Flags . autonav = 0; 

/* Calculate Kp coefficient for turning. Define MAX-TURN above */ 

K.P.coefficient = pow (((( double ) MAX_TURN_DIFF) / 8 0.0) , (1.0/3.0)); 

/* End ROS Initializations */ 

while ( ros : : ok ()) 

{ 

ros :: spinOnce (); // Callback to all active topics 

/* Begin Navigation */ 

i f ( Cmd_Flags . autonav == 1) 

{ 

nav_val = Navigate(); 

if(nav_val == 1) // Waypoint reached, inform and stop MONTe 

{ 

pub-flags . auto.nav = Cmd.Flags . autonav ; 
pub-flags . nav.mode = ’N’; 

pub.flags . incoming.route = Cmd.Flags . route ; 

// N indicates waypoint reached, need next waypoint 
cmd.fig.pub . publish ( pub.flags ); // Publish to cmd-flags topic 

cmd.left = (unsigned char) STOP//Send stop command to PlantControl 
cmd. right = (unsigned char) STOP-K\ // while waiting for next waypoint 
pit.cmd.pub . publish (cmd); 

} 

else // Send command signal to plant to move towards waypoint 

{ 

cmd.left = (unsigned char) left.command ; 
cmd. right = (unsigned char) right.command; 
plt.cmd.pub . publish (cmd); 

} 

} // End Navigation 

loop.rate . sleep (); // Sleeps to maintain loop-rate 

} // end while loop 

} //end main 

Function: Command-FlagsCallback 

Description: Pulls waypoint from the Command-Flags-T topic 

Parameter: 1) Pointer to ROS message from topic Command-FlagS-T 

Return Value: None 

void Command-FlagsCallback (const MONTe:: Command-FlagsConstPtr& f lag s ) 

{ 
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Cmd_Flags . autonav = flags—> auto_nav ; 

Cmd.Flags . mode = flags—> nav.mode; 

Cmd.Flags . route = flags—> incoming-route; 

} // end callback 

Function: WaypointCallback 

Description: Pulls waypoint from the Current .Waypoint _T topic 

Parameter: 1) Pointer to ROS message from topic Current .Waypoint _T 

Return Value: None 

void WaypointCallback ( const MONTe: : WaypointConstPtr& way_pt) 

{ 

Current_WP . 1 at = way_pt—>latitude ; 

Current_WP . Ion = way_pt—>longitude ; 

} // end callback 

Function : N avigati on .Data Callback 

Description: Pulls waypoint from the Current.Waypoint.T topic 

Parameter: I) Pointer to ROS message from topic Current .Waypoint .T 

Return Value: None 

void Nav_DataCallback ( const MONTe: : Nav_DataConstPtr& nav_dat) 

{ 

Current_Nav_Data . lat = nav_dat—>latitude ; 

Current_Nav_Data . Ion = nav_dat—>longitude ; 

Current_Nav_Data . heading = nav_dat—>heading ; 

} // end callback 

Function: Navigate 

Description: Navigate MONTe. Pulls current posit and waypoint. Determines 

range and heading to desitiantion . Calls plant control function to 
produces plant command. 

Parameter: None 

Return Value: 0 — Success 

int Navigate () 

{ 

static double lat , Ion, *lat_ptr , *lon_ptr; // Current position 

static double wlat , wlon , *wlat_ptr, *wlon_ptr; // Waypoint position 
static double lat.diff, lon.diff, *lat_diff_ptr, *lon_diff_ptr; 
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// variables to calculate differences in latitude & longitude 
static double rng , *rng_ptr; //Range (in yards) 

static float cur.hdg , new.hdg , * cur.hdg.ptr , *new_hdg_ptr; 

/* Pointer initializations */ 

lat.ptr = &lat ; 
lon.ptr = &lon ; 
wlat.ptr = &wlat ; 
wlon.ptr = &wlon ; 
rng.ptr = &rng ; 
lat_diff_ptr = &lat_diff; 
lon_diff_ptr = &lon_diff; 
cur_hdg_ptr = &cur_hdg ; 
new_hdg_ptr = &new_hdg ; 

/* Update variables */ 

*lat_ptr = Current_Nav_Data . lat ; 

*lon_ptr = Current_Nav_Data . Ion ; 

*wlat_ptr = Current.WP . lat ; 

* wlon.ptr = Current.WP . Ion ; 

*cur.hdg.ptr = Current.Nav.Data . heading ; 

/* Compute range to current waypoint. */ 

*rng_ptr = sqrt((((2000 * (* w 1 at _ptr )) — (2000 * 

(* 1 at _p tr )))* ((2000 * (* wlat _ptr )) —(2000 * (* 1 at _p tr ))))+ 
(((1600 * (* wlon _ptr)) — (1600 * (wlon.ptr)))* 
((1600 * (* wlon.ptr))— (1600 * (* 1 on _ptr ))))); 

if (* rng.ptr <= RANGE.THRESH)// close enough to waypoint , action 
{ //code takes effect and next waypoint is loaded 

return 1; 

} 


// 3600 converts lat^diff and lon^diff to decimal seconds for accuracy 

* 1 at _ d i f f _p tr = 3600 * ((* wlat_ptr) —(* 1 at _p tr )); 

* 1 o n .d i f f. p t r = 3600 * ((*lon.ptr) — (* wlon.ptr )); 

// Compute new.hdg using the differences in lat/long 
*new.hdg.ptr = atan2 (* lon.diff .ptr , * 1 at .diff .ptr )* 1 80/PI; 

// Convert quadrant III/IV degrees to 180 — 360 
if (* new.hdg.ptr < 0.0) 

*new.hdg.ptr += 360.0; 

Plant.Compensator(* rng.ptr , * new.hdg.ptr , * cur.hdg.ptr ); 
return 0; 

} // end Navigate 

Function: Plant-Compensator 
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Description: PID control for MONTe. P & D control turning rate. I 

maintains forward speed. Checks to keep wheels moving forward so no 
tank turns happen. Also, limits maximum turn rate to avoid spinning . 

Parameter: 1) Range to waypoint 

2) Desired heading to waypoint 

3) Current heading of MONTe 

Return Value: None 

int Plant.Compensator ( double range, float desired.hdg , float current.hdg) 

{ 

float hdg_error ; 
unsigned char k_p_left = 0; 
unsigned char k_d_left = 0; 
unsigned char k_p_right = 0; 
unsigned char k_d_right = 0; 
unsigned char k_i = 0; 

hdg.error = desired.hdg — current.hdg ; 

if (hdg.error > 180.0) 

hdg.error —= 360.0; 
else if (hdg.error <= —180) 
hdg.error += 360; 

/* Proportional & Derivative control with gain scheduling */ 

/* Max turn if outside of +\— 80 degrees */ 

if (( hdg.error < 80.0) || (hdg.error > 80.0)) 

{ 

k_p_left = —MAX_TURN_DIFF * (char) hdg.error / (char) fabs (hdg.error ); 
k.p.right = MAX.TURNJDIFF * (char) hdg.error / (char) fabs (hdg.error ); 

} else 

{ // Proportional gains for turning , in form of k^p = A^x''3 

k.p.left = (char) (—K.P.coefficient * pow( hdg.error , 3.0)); 
k.p.right = (char) (K.P.coefficient * pow( hdg.error , 3.0)); 

// Derivative gain for turning 

} 

/* Integral control for maintaining velocity */ 

/* Assign speeds and provide limiting */ 

left.command = FWD_SPEED + L.CORRECT + k.p.left + k.i + k.d.left; 
right.command = FWD_SPEED + R.CORRECT + k.p.right + k.i + k.d.right; 

/* Keep commands inside design limitations incase of data corruption */ 

if (left.command > 117.0) // Design speed range for left side 

left.command = 117.0; // 65—117 (64 stop, 127 full) 

else if (left.command < 65.0) 
left.command = 65.0; 
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if ( right_command > 246.0) // Design speed range for right side 

right_command = 246.0; // 191—246 (190 stop, 256 full) 
else if (right-command < 191.0) 
right-Command = 191.0; 

return 0; 

} // end Plant .Control _Func 


C.2 Waypoint Processing Node 


Following code processes waypoint routes for MONTe. 


Title: MONTe.Waypoint.Processing 0.0 (ROS Node) 

Author: Jason Hickle 

Purpose: Receives , store and publishes waypoint data for use in autonomous 

navigation . 

Handles the following functions : 

1) Monitor Command.Flags to determine behavior. 

2) Receive waypoints from New.Waypoint and store them. 

3) Publish current waypoint for use in Navigation . 

Use: Interface between user and navigation node. When CF. route equals 

"true,” program proceeds to receive and process a waypoint router. 

Can work with any other node that publishes to ” New .Waypoints .T. ” 
Receives waypoints (up to 10) and stores for sending waypoints to the 
Navigation node. Will receive waypoints from Waypoint .Control (user 
input over VNC), or via Comms (wireless comms from base station) 

Waits for nav.mode ’N’ to send new waypoint to navigation mode. 
Receives from Command.Flags. 

Node set up to allow future capability . Possible functions could 
be to set up search types , additional actions to perform upon reaching 
waypoint , etc . 

Loop rate set at 4 Hz. This synchs with system loop rate. 


ROS Notes: 

Name- 

Publications — 

Subscriptions — 

Messages— 


” MONTe .Waypoint .Processing 

” Current .Waypoint .T ” 

” Command.Flags.T” 

” New .Waypoints .T ” 

” Command.Flags .T” 

Waypoint. msg 
Command.Flags. msg 
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Services — 


None 


Version History: 

- Version 0.0 - 

May 22nd, 2011 
LT Jason Hickle 

Establishes basic functionallity . Receives , stores and sends 
waypoints based off of Command_Flags_T 

Notes: Further information can be found on ROS Wiki page: 

http : //www. ros . org / wiki / 

/* Libraries */ 

#include <ros/ros.h> 

#include ’’MONTe/Waypoint. h” // Message format (.msg) 

#include ’’MONTe/Command.Flags . h” // Message format (.msg) 

/* D efi nes */ 

// Debugging options , uncomment to enable 

#define WPJNCOMING // Publish info to ROS for incoming waypoints 

#define WP_SEND // Alert ROS when a new waypoint is sent 

/* CF holds all command flags. Expand as necessary. Ensure publishing 

/ utilizes default values for any flag unchanged to ensure flags are 

/ not being changed unecessarily. 

typedef struct 

{ 

bool autonav; 
char mode; 
bool route; 

}CF; //Define flags structure 

/* WPJ* stores all data necessary to navigate to, and determine 

/ behavior mode for a route. 

typedef struct 

{ 

int num; 
double 1 at ; 
double Ion ; 
char action ; 

}WP_P; //Define waypoint structure 

/* Global Variables */ 

WP_P New_WP, waypoints [ 1 0]; 

CF Cmd-Flags ; 



int route_points = 0; // Stores length of route 


/* Functional Prototypes */ 

void Command_FlagsCallback( const MONTe:: Command_FlagsConstPtr& flags ); 

void WaypointCallback ( const MONTe: : WaypointConstPtr& way.pt); 

int main(int argc , char **argv) 

{ 

/* ROS Initializations */ 

ros : : i n i t ( argc , argv , ”MONTe_Waypoint_Processing” ); // Set up ROS node 

ros : : NodeHandle n; // set up handle for this node 
// Set up all publishers for node 

ros :: Publisher nxt_wp_pub = n . adverti se <MONTe:: Waypoint >(” Current_Waypoint_T ” , 1); 

ros :: Publisher cmd_flg_pub = n . advertise <MONTe: : Command_Flags >(”Command_Flags_T” , 1) 

// Set up all subscriptions for node 

ros :: Subscriber sub.cf = n . subscribe (”Command.Flags.T” , 1, Command.FlagsCallback ); 
ros :: Subscriber sub.wp = n . su b s cribe (”New_Waypoint_T” , 1, WaypointCallback); 

ros:: Rate lo o p .rate (4); // Sets 4hz cycle for main loop 

// Set up message handles for communicating with topics. 

MONTe:: Command.Flags flags , pub.flags ; // subscribe and publiser handles 
MONTe:: Waypoint new.way.pt , next.way.pt; // subscribe and publiser handles 

// int total ^wp ^counter = 0; 

// int wp-entry ^counter = 0; 
int current.wp.number = 0; 

pub.flags . auto.nav = 0; // Initialize system in manual control, enroute to 

pub.flags . nav.mode = ’A’; // next waypoint , and no new route 
pub.flags . incoming.route = 0; 
cmd.flg.pub . publish (pub.flags ); 

while ( ros : : ok ()) 

{ 

ros : : spinOnce (); // Callback to all active topics 

// Check for new route and fill in waypoints 
if (Cmd.Flags . route == 1) 

{ // Check for errors and fill in new waypoint 

if ((New.WP.num >= 0) && (New.WP.num < 10)) 

{ // Populate the waypoint queue 

waypoints [New.WP.num]. num = New.WP.num; 
waypoints [New.WP.num]. 1 at = New.WP. lat; 
waypoints [New.WP.num]. Ion = New.WP. [on; 
waypoints [New.WP.num]. action = New.WP. action ; 
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#ifdef WPJNCOMING 

ROS_INFO(”New WP: #%d , Lat- %lf , Lon- %lf , Action- %c. 
Route Size— %d” , waypoints [New_WP. num]. num, waypoints [New_WP. num ]. lat , 
waypoints [New_WP. num ].lon, waypoints [New_WP. num ]. action, route.points); 

#endif 

} 

// End of route received , place in ”no new route” 

i f (New.WP. num == (route.points — 1)) 

{ 

pub.flags . incoming.route = 0; 
pub.flags . auto.nav = Cmd.Flags . autonav ; 
pub.flags . nav.mode = Cmd.Flags . mode ; 
cmd.flg.pub . publish (pub.flags ); 

} 

} // End New Route if 

// Send waypoint if auto nav and next waypoint 
// check for auto^nav and if new waypoint is needed 
if (( Cmd.Flags . autonav == 1) && (Cmd.Flags . mode == ’N’)) 

{// New waypoint is available 

if (( current.wp.number < route.points) && ( current.wp.number >= 0)) 
{ // Send next waypoint 

next.way.pt . latitude = waypoints 1 current.wp.number ]. lat ; 
next.way.pt . longitude = waypoints 1 current.wp.number ]. Ion ; 
next.way.pt . action = waypoints [ current.wp.number ]. action ; 

nxt.wp.pub . publish (next.way.pt ); 

#ifdef WP_SEND 

ROS_INFO(”Waypoint %d sent to Nav.”, current.wp.number); 
#endif 

current_wp_number++; 

} else // Route complete , reset waypoints 

{ 

pub.flags . auto.nav = 0; 
pub.flags . nav.mode = ’A’; 

pub.flags . incoming.route = Cmd.Flags . route ; 
cmd.flg.pub. publish(pub.flags ); 

#ifdef WP_SEND 

ROS_INFO(”Route complete, in Manual Control.”); 
#endif 

current.wp.number = 0; 

} // End Send wp/ route complete 

} 

loop.rate . sleep (); // Sleeps to maintain loop^rate 

} // End main while 
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} //end main 


Function : Command-FlagsCallback 

Description: Pulls waypoint from the Command .Flags _T topic 


Parameter: 1) Pointer to ROS message from topic Command .Flags _T 


Return Value: None 

void Command_FlagsCallback( const MONTe: : Command_FlagsConstPtr& flags ) 

{ 

Cmd_Flags . autonav = flags—>auto_nav ; 

Cmd_Flags . mode = flags—>nav_mode ; 

Cmd_Flags . route = flags —>incoming_route ; 

} // end callback 


Function: WaypointCallback 


Description: Pulls waypoint from the Current .Way point .T topic 


Parameter: 1) Pointer to ROS message from topic Current.Waypoint.T 


Return Value: None 

void WaypointCallback ( const MONTe:: WaypointConstPtr& new.way_pt) 

{ 

New_WP.lat = new_way_pt—>latitude ; 

New_WP. Ion = new_way_pt—>longitude ; 

New_WP. action = new_way_pt—>action ; 

New_WP.num = new_way_pt—>wp_num; 
route_points = new_way_pt—>route ; 

} // end callback 


C.3 Monkey Driver 

Following driver interfaces with Monkey board. 

Title: MONTe.Monkey.Driver 0.0 (ROS Node) 

Author: Jason Hickle 

Purpose: Node that allows MONTe to interface with CHIMU AHRS ("Monkey”). 

Handles the following functions : 

1) Read in serial data from Monkey AHRS. 

2) Parse data string. 

3) Publish data to Nav.Data topic. 
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Use: 


Driver for Monkey nav unit. 

Output from Monkey needs to be the following comma separated values (CSV): 
$RM 

Satellites Tracked 
Fix Quality 
Latitude 
Longitude 
Heading 
Altitude 
Velocity (N/S) 

Velocity (E/W) 

Velocity (D/U) 

Additional parameters can be included by expanding the parsing function . 

Runs at loop rate of 4 Hz. This is MONTe’s program system loop 
rate . 

ROS Notes: 

Name— "MONTe.Monkey” 

Publications — ” Command .Flags _T” 

"Nav.Data.T” 

Subscriptions — None 

Messages— Command.Flags.msg 

Nav .Data . msg 

Services — None 

Version History: 

- Version 0.0 - 

Mar list , 2011 
LT Jason Hickle 

Established link with Monkey at 115200 Baud. Code in place to 
receive full data string (waiting on adjustment of code on Monkey 
board itself). Successfully transmit nav data to "Nav.Data.T.” 

Notes: Further information can be found on ROS Wiki page: 

http : //www. ros . org / wiki / 

/* Libraries */ 

#include <ros/ros.h> 

#include ”MONTe/Nav_Data. h” // Message format (.msg) 

#include ”MONTe/Command_Flags . h” // Message format (.msg) 

#include ”MONTe_USB_Serial_Lib . h” 
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/* D efi nes * / 

// Debugging options , uncomment to enable 

//^define NAV-PRINT // Print out nav data and check for successful 

// parsing of command string 

NMEAJ)ATA is the data structure to hold navigational data received / 

from the Monkey/AHRS. Listed in order of data receieved via command / 

string. Use this to expand MONTe’s capabilities for processing data. */ 

typedef struct { 

unsigned char command.str [ 3 ]; 
int sat.track ; 
int fix_quality ; 
double latitude ; 
double longitude ; 
double heading ; 
int altitude ; 
double vel_N ; 
double vel_E ; 
double vel_D ; 

} NMEAJ)ATA; 

/* Global Variables 

NMEAJDATA GPS .Buffer; 
char nav_buffer[254]; 

/* Functional Prototypes 

int Parse.Monkey.Data (); 

void Print.Nav.DataO; 

int main(int argc , char **argv) 

{ 

int usb.fd ; // File descriptor for serial comms, included for future use 

// for error checking 

/* ROS Initializations */ 

ros : : i n i t ( argc , argv , ”MONTe_Monkey” ); // Set up ROS node 

ros : : NodeHandle n; // set up handle for this node 

// Set up all publishers for node 

ros :: Publisher nav.pub = n . adverti se <MONTe:: Nav .Data >(” Nav.Data.T ” , 1); 

ros :: Publisher cmd.flg.pub = n . advertise <MONTe: : Command.Flags >(”Command.Flags.T” , 1) 

ros:: Rate lo o p .rate (4); // Sets 4hz cycle for main loop 

MONTe: : Nav.Data n.data ; 

MONTe: : Command.Flags flags ; 

/* End ROS Initializations */ 


*/ 


*/ 


// Buffer for nav data incoming from Monkey 


/* 

/ 

/ 
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usb_fd = OpenUSBSerialPort (); 


while ( ros : : ok 0 ) 

{ 

// Receive Navigation data 

// Update Command.Flags with fix quality (future implementation) 
if ( ReadfromUSBSerialPort ( nav.buffer , 254) > 0) 

{ 

#ifdef NAV_PRINT 


#endif 


printf (”\nStep 1”); 


// Parse string data 
Parse_Monkey _Data (); 


#ifdef NAV_PRINT 


#endif 


Print_Nav_Data (); 


#ifdef NAV_PRINT 


#endif 


printf (”\nStep 10\n”); 


/* Update navigation data for publishing */ 

n.data. latitude = GPS .Buffer, latitude; 
n.data . longitude = GPS.Buffer . longitude ; 

// n _data . heading = GPS .Buffer . heading ; 

/* Publish navigation data to topic */ 

nav.pub . publish (n.data ); 

} // end if 

loop.rate . sleep (); // Sleeps to maintain loop.rate 

} // end while loop 

CloseUSBSerialPort (); 

return 0; 

} //end main 

Function: Parse .Monkey .Data 

Description: Tokenizcs buffer to populate navigation data. Add or subtract 

steps to adjust what gets pulled from string. 


Parameter: None 


Return Value: 0— Success 

int Parse.Monkey.Data 0 

{ 

static char *buf_ptr; 
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#ifdef 

#endif 

#ifdef 

#endif 

#ifdef 

#endif 

#ifdef 

#endif 


#ifdef 

#endif 


#ifdef 

#endif 

#ifdef 

#endif 


buf-ptr = &nav- buffer [4]; // Start pointer at 4th element (satillites tracked) 

NAV-PRINT 

p r i n t f (”\nS tep 2”); 

NAV-PRINT 

p r i n t f (”\nS tep 3”); 

NAV-PRINT 

print! (”\nStep 4”); 


GPS-Buffer . sat-track = atoi ( strtok ( buf_ptr , 

NAV-PRINT 

print! (”\nStep 5” ); 

//GPS-Buffer . fi X-q u a I i ty = atoi ( strtok (NULL, 

// Converts ascii fix quality to an integer 

GPS-Buffer . latitude = atof ( strtok (NULL, / 10000; 

// Converts ascii latitude to decimal degrees 

NAV-PRINT 

p r i n t f (”\nS tep 6”); 


GPS-Buffer . longitude = atof ( strtok (NULL, ”,”)) / 10000; 

// Converts ascii longitude to decimal degrees 

//GPS-Buffer . heading = atof ( strtok (NULL, ”,”)) / 10; 

// Converts ascii heading to double with 2 decimal places 

GPS-Buffer . altitude = atoi ( strtok (NULL, 

// Converts ascii altitude to integer meters (MSL) 

NAV-PRINT 

p r i n t f (”\nS tep 7”); 


GPS-Buffer . vel-N = at o f ( s t rt o k (NULL, ”,”)) / 10; 

// Converts ascii N/S velocity to double with 2 decimal places 

NAV-PRINT 

p r i n t f (”\nS tep 8”); 


GPS-Buffer . vel-E = at o f ( s t rt o k (NULL, ”,”)) / 10; 

// Converts ascii EPW velocity to double with 2 decimal places 
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#ifdef NAV_PRINT 


#endif 


printf (”\nStep 9”); 


GPS_Buffer . vel.D = ato f ( s trto k (NULL, / 10; 

// Converts ascii U/D velocity to double with 2 decimal places 

buf _ptr = NULL; 

return 0; 

} // end callback 

Function: Print ^Nav ^Data 

Description: Printing function for troubleshooting . Uncomment/add lines 

to print out additional data. 

Parameter: None 

Return Value: None 

void Print_Nav_Data() 

{ 

printf (”\n\tCommand string \ t%s\n” , GPS.Buffer . command.str ); 
printf(” Satellites tracked \ t%d\n” , GPS.Buffer. sat.track); 

// printf (”\n\tFix quality\t%s\n”, GPS.Buffer . fix .quality ); 
printf (”\tCurrent Latitude \ t%f \n” , GPS.Buffer. latitude); 
printf (”\tCurrent Longitude \ t%f\n” , GPS.Buffer. longitude); 

// printf (”\tCommand String\t%f\n", GPS.Buffer . heading ); 
printf (”\tCommand String\t%d\n” , GPS .Buffer . a 11 i tu d e ); 
printf (”\tCommand S tring \ t%f \n” , GPS.Buffer.vel.N); 
printf (”\tCommand String\t%f\n” , GPS.Buffer . vel.E ); 
printf (”\tCommand S tring \ t%f \n” , GPS.Buffer.vel.D); 

} // end callback 


C.4 USB-Serial Library 

Library for running RS-232 serial communications over USB port. Used in conjunction with 
MONTeJVlonkeyJDriver.cpp. 

Title: MONTe.USB.Serial.Lib 0.0 

Author: Jason Hickle 

Purpose: List of functions to read and write serial data over a USB—Serial 

for use with MONTe on Linux. 
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Use: Include "MONTe.USBSerial.Lib . h” in the header of each node needed. 

Include source file as part of CMakelist. txt as follows: 
rosbuild.add.executable (node .name src/node.name . cpp src / MONT e .USB .Serial .Lib . cpp) 

Verify that serial handle (/ dev / ttyUSBO) is correct for system. 

Version History: 

- Version 0.0 - 

Mar list , 2011 

LT Jason Hickle 

Open and Close USB/ Serial port. Reads data from serial port. 

/* Libraries * / 

#include <stdio.h> 

#include <unistd.h> 

#include <sys / types . h> 

#include <sys/stat.h> 

#include <fcntl.h> 

#include <termios.h> 

#include <string.h> 

#include <errno.h> 

#include ”MONTe_USB_Serial_Lib . h” 


/* 

D efi nes 

*/ 

/* 

Global Variables 

*/ 

static 

int fd = 0; 


static 

struct termios oldtio ; 


/* 

Functions 

*/ 


Function: OpenUSBSerialPort 

Description: Takes port number and opens appropriate serial connection. 

Will save old port data 

Parameter: I) sPortNumber — pointer to comm port ttyS(X}, ie 0 for ttySO 

etc . 

Return Value: fd — file descriptor for port 

int OpenUSBSerialPort 0 

{ 

char sPortName [64] = ”/dev/ttyUSBO”; // Hardcoded until generic method works 

// make sure port is closed 
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CloseUSBSerialPort (); 

fd = open(sPortName , 0_RDWR | O-NOCTTY | OJ«)ELAY); 
if (fd < 0) 

{ 

printf(”open error %d %s\n”, errno, strerror(errno)); 

} 

else 

{ 

struct termios my.termios ; 
t c g e t a 11 r (fd , &my .termios ); 

oldtio = my.termios; // Save port attributes to restore later 
tcflush(fd, TCIFLUSH); 

my.termios . c.cflag = B115200 | CSS | CREAD | CLOCAL | HUPCL; 

cfsetospeed(&my_termios , B115200); 
t c s e t a 11 r (fd , TCSANOW, &my .termios ); 

} // end if 

return fd ; 

} // end K.OpenSerialPort 

Function: CloseUSBSerialPort 

Description: Checks to see if port is open and then closes it. Returns 

port attributes to original configuration . 

Parameter: None 

Return Value: None 

void CloseUSBSerialPort 0 

{ 

// you may want to restore the saved port attributes 
if (fd > 0) 

{ 

tcsetattr (fd , TCSANOW, &oldtio); 
close(fd ); 

} // end if 

} // end K_CloseSerialPort 

Function: WritetoUSBSerialPort 

Description : 

Parameter: I) 
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Return Value : 

int WritetoUSBSerialPort(char* psOutput) 

{ 

int iOut ; 

if ( fd < 1) // Port is not open, return —1 

{ 

return —1; 

} // end if 

iOut = write(fd, psOutput, 1); // Set to I so only one byte is xmitted 

if (iOut < 0) 

{ // Place in ROSJNFO statement !!!!!!! 

//printf write error %d%s\n", errno , s t r e r ro r ( e rrno )); 

} 

return iOut ; 

} // end K -Write to S e rialPo rt 

Function : ReadfromU SB Serial Port 
Description : 

Parameter: 1} 

Return Value : 

int ReadfromUSBSerialPort ( char* psResponse , int iMax) 

{ 

int iln ; 

// printf (” in ReadAdrPort iMax=%d\n’', iMax); 

if (fd < 1) 

{ 

printf(” port is not open\n”); 
return —1; 

} // end if 

strncpy (psResponse, ”N/A” , iMax<4?iMax : 4); 
iln = read(fd, psResponse, iMax —1); 

if ( iln < 0) 

{ 

if (errno == EAGAEsT) 

{ 

return 0; // assume that command generated no response 

} 

else 
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{ 


printf(”read error %d %s\n”, err no, strerror(errno)); 
} // end if 

} 

else 

{ 

psResponse [ iln <iMax? iln : iMax] = ’\0’; 

// print/(" read %d chars: %s\n”, iln, psResponse); 

} // end if 

return iln ; 

} // end ReadAdrPort 


Title: MONTe.USB.Serial.Lib 0.0 (ROSNode) 

Author: Jason Hickle 

Purpose: Header file for MONTe serial comms. 

/* Libraries * / 

#include <ros/ros.h> 

int OpenUSBSerialPort (); 

void CloseUSBSerialPort (); 

int WritetoUSBSerialPort( char* psOutput ); 

int ReadfromUSBSerialPort ( char* psResponse, int iMax); 


C.5 Plant Control Driver 

Following driver interfaces with Sabertooth 2x12 Motor Drivers. 

Title: MONTe _Plant ^Control 0.0.4.2 (ROSNode) 

Author: Jason Hickle 

Purpose: Driver node that allows MONTe to control the whegs and water jets. 

Handles the following functions : 

1) Manual Control 

2) Communicate with Sabertoothlxl 2 motor drivers 

Use: Receives command data from the command topics and executes. 

Commands sent to Sabertooth2xl 2 motor driver via RS—232 interface , 
over port /dev/ttySO. Can run both whegs and waterjets over same 
interface as long as commands have correct address in packetizcd 
serial mode. 

For plant control running in Simplified Serial mode for Sabertooth 
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2x12 motor drivers: Each plant command will send 4 bytes of data. 

1st byte: Left motor command (1 — full rev, 64—stop , 127—full fwd). 

2nd Byte: Left motor command (128 —full rev, 190—stop, 256—full fwd). 


For plant control running in Packetized Serial mode for Sabertooth 
2x12 motor drivers: Each plant command will send 4 bytes of data. 

1st byte: motor controler address. 

2nd Byte: command (fwd, rev, left turn, right turn) 

3rd Byte: speed (0 — 127) 

4th Byte: checksum , (address+command+speed) & ObOlllllll 

Manual control receives desired speed and turn rate from topic , and 
parses data. Converts signed intS^t to unsigned char for xmit to 
motor controllers . 

Runs at system loop rate of 4 Hz. 


ROS Notes: 

Name— ” MONTe -Plant-Control" 

Publications— None 

Subsrciptions — ” Plant-Commands-T” 

Messages— Plant-Command. msg 

Services — None 

Version History: 

- Version 0.0.4.2 - 

Apr 26th , 2011 
LT Jason Hickle 

— Enable simplified serial control for Sabertooth2xl 2 . Packetized serial is problematic 

- Version 0.0.4.1 - 

Apr 26th , 2011 
LT Jason Hickle 


— Enable packetized serial control for Sabertooth2xl 2 . Using alternate logic 
for parsing data commands. Instituted a Ihz spin rate for sending commands. 
Using new message (v2) for sending command data. Moderate success, sends right 
commands for a command or two, then abberent behavior occurs. 

- Version 0.0.4 - 

Apr 26th , 2011 
LT Jason Hickle 


— Enable packetized serial control for Sabertooth2xl 2 . Non—deterministic behavior. 
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Version 0.0.3 


Apr 26th , 2011 
LT Jason Hickle 

— Enabled simplified serial control for Sabertooth2xl2 . Successful 

- Version 0.0.2 - 

Mar 31 St , 2011 
LT Jason Hickle 

— Successfully receives commands from control topic , and correctly 
parses them. Unable to get packetized serial to work with Sabertooth2xl2 
motor controllers . Will persue simplified serial and come back to 
packetized serial later. 

- Version 0.0.1 - 

Mar 31st , 2011 
LT Jason Hickle 

— Successfully receives commands from control topic , and correctly 
parses them. 

- Version 0.0 - 

Mar 21st , 2011 
LT Jason Hickle 

Enable Manual Control mode in main control loop. 

Notes: Further information can be found on ROS Wiki page: 

http : //www. ros . org / wiki / 

/* Libraries */ 

#include <stdio.h> 

#include <stdlib.h> 

#iiiclude <unistd.h> 

#include <sys / types . h> 

#include <sys/stat.h> 

#include <fcntl.h> 

#include <termios.h> 

#include <string.h> 

#include <errno.h> 

#include <stdint.h> 

#include <unistd.h> 

#include <ros/ros.h> 

#include ” std_msgs / S tring . h” 

#include ”MONTe/Plant_Command . h” // Message format (.msg) 

#include ”MONTe_Serial_Lib .h” 

#include <sstream> 

/* Debugging */ 
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// Debugging options , uncomment to 

enable 






#define MOTOR_CONTROL_DEBUG 

// 

Prints 

out 

commands from 

manual cmd 

topic 

//^define FLOW.CHECK 

// 

Prints 

out 

stages of code 

, uncomment 

to check that 


// 

indivia 

lual 

sections are running 



/* D efi nes * / 

const char MOTOR_PORTJS[UMBER = 0; // For adding ttyS(X) selection later 

/* Global Variables */ 


// Control Variables 


uint8_t 

MONTe_Left, MONTe.Right; 

// 

uint8_t 

temp.left = 0; 

// 

uint8_t 

temp.right = 0; 

// 


Received data from Manual_Commands_T 
temporary storage values to check for 
if manual commands have changed 


/* Functional Prototypes */ 

void Plant_CommandCallback ( const MONTe:: Plant_CommandConstPtr& command); 
int Manual_Control_Simplified_Parser (); 

int PI ant _C ontro 1 _S i mp 1 i fi ed ( unsigned char* s.speed , const char* port.num); 

int main(int argc , char **argv) 

{ 

/* Variable s */ 

// Flags 

bool Manual_Command_F = 1; 

//bool New-Command^F = 1; 

/* Initializations * / 

// Perform initializations for ROS 

ros : : init ( argc , argv , ”MONTe.Plant.Control” ); // Set up ROS node 

ros : : NodeHandle n; // set up handle for this node 

// Set up all subscriptions for node 

ros :: Subscriber man.cmd = n . subscribe (”Plant.Command.T” , 100, Plant.CommandCallback ) 

ros::Rate 1 oop .rate (4); // Sets 4hz cycle for main loop 

MONTe:: Plant.Command command; 

// Initialize variables 
MONTe.Left = 0; 

MONTe.Right = 0; 

char portno = (char) MOTOR_PORT_NUMBER; 

K_OpenSerialPort(&portno ); 

/* Main control loop. * 

* Performs the following: — Poll all topics * 
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— Manual control 

— Maintains desired loop rate 


*/ 

while ( ros : : ok ()) 

{ 

ros :: spinOnce (); // Callback to all active topics 

#ifdef MOTOR_CONTROLX)EBUG 

ROS_INFO(” Topic data: Left Speed %d\tRight Speed %d” , MONTeXeft, MONTe.Right); 

#endif 


i f (ManuaLCommand.F) 


{ 

} 


Manual_Control_Simplified_Parser(); 


loop_rate . sleep (); // Sleeps to maintain loop^rate 

} // end while loop 

K.CloseSerialPort (); 

} //end main 


Function : Plant_CommandCallback 


Description: Pulls manual commands from the Plant .Command topic 

Parameter: 1) Pointer to ROS message from topic Plant .Command 

Return Value: None 

void Plant_CommandCallback ( const MONTe:: Plant_CommandConstPtr& command) 

{ 

MONTe.Left = command—>left ; 

MONTe.Right = command—>right ; 

} // end callback 

Function : Manual .Control .Simplified _P ars er 

Description: Pulls manual control data for forward / reverse speed and 

turning rate. Calls Plant .Control to send commands to Sabertooth 
2x12 motor drivers . 


Parameter: Void 


Return Value: 0 No new command 

1 New command sent 


int Manual_Control_Simplified_Parser() 
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{ 


unsigned char sabertooth.left.speed ; // Desired left speed. 

unsigned char sabertooth.right.speed ; // Desired right speed 

/* Check if new command is received. Return 0 if not. */ 

if ((MONTeXeft == temp.left) && (MONTe.Right == temp.right)) 

{ 

return 0; 

} 

#ifdef MOTOR_CONTROLJ)EBUG 

ROS_INFO(”Current : Left-9kl, Right-%d\nOrdered: Left-Trd, Right-%d.”, 
temp.left, temp.right, MONTe.Left, MONTe.Right); 

#endif 

sabertooth.left.speed = (unsigned char) MONTe.Left; 
sabertooth.right.speed = (unsigned char) MONTe.Right; 

/* Send command to left motor. */ 

Plant_Control_Simplified(& sabertooth.left.speed , &MOTORJ\)RT3JUMBER); 

/* Send command to right motor. */ 

Plant.Control.Simplified(& sabertooth.right.speed , tSMOTORJGRTJMUMBER) 

temp.left = MONTe.Left; 
temp.right = MONTe.Right; 

return 1; 

} // end ManuaCCommand^Parser 


Function: Plant ^Control 

Description: Takes a command string and transmits to Sabertoothlxl 2 

motor drivers via RS — 232. Opens port, writes address /command 
/ data / checksum , then closes port. This is for simplified 
serial mode. 


Parameter: 1) Pointer to speed command 

2) Port number of serial eg 0 of ”ttyS0” 

Return Value: 0 Command sent successfully 

—1 Port failed to open 

—2 Write failed 

int PI ant .C ontro 1 .S i mp 1 i fi ed ( unsigned char* s.speed , const char* port.num) 

{ 

char port ; 

port = (char) *port.num; 


// if ( K-OpenSerialPort{&port )<0) 
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// return —1; 


if ( K-WritetoSerialPort ( s_speed)<0) 

return —2; 

//K.CloseSerialPort {): 

return 0; 

} 


C.6 Serial Library 

Library for running RS-232 serial communications over USB port. Used in conjunction with 
MONTe .Plant .Control. cpp. 

Title: MONTe .Serial _Lib 0.0 

Author: Jason Hickle 

Purpose: List of functions to write serial data for use with MONTe on Linux 

Use: Include " MONTe Serial-Lib . h” in the header of each node needed. 

Include source file as part of CMakelist. txt as follows: 
rosbuild-add-executable (node-name src / node-name . cpp src / MONTe Serial-Lib . cpp ) 

Verify that serial handle (/dev/ttySO) is correct for system. 

Version History: 

- Version 0.0 - 

Mar list , 2011 
LT Jason Hickle 

Open and Close Serial port. Write data to serial port. 

/* Libraries * / 

#include <stdio.h> 

#include <unistd.h> 

#include <sys/types.h> 

#include <sys/stat.h> 

#include <fcntl.h> 

#include <termios.h> 

#include <string.h> 

#include <errno.h> 

#include ”MONTe_Serial_Lib .h” 

/* Defines */ 
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/* Global Variables */ 

static int fd = 0; 

static struct termios oldtio ; 

/* Functions */ 

Function: K_OpenSerialPort 

Description: Takes port number and opens appropriate serial connection. 

Will save old port data 

Parameter: 1) sPortNumber — pointer to comm port ttyS(X}, ie 0 for ttySO 

etc . 

Return Value: fd — file descriptor for port 

int K-OpenSerialPort (char* sPortNumber) 

{ 

char sPortName [64] = ”/dev/ttySO”; // Hardcoded until generic method works 

// sprintf (sPortName , ”/dev /1 ty S%c ”, ^sPortNumber); // Not working, need to research 

// make sure port is closed 
K.CloseSerialPort (); 

fd = open (sPortName , 0_RDWR | 0_N0CTTY | OJMDELAY); 
if (fd < 0) 

{ 

printf(”open error %d %s\n”, errno, strerror(errno)); 

} 

else 

{ 

struct termios my_termios ; 
t c g e t a 11 r (fd , &my .termios ); 

oldtio = my.termios ; // Save port attributes to restore later 

tcflush(fd, TCIFLUSH); 

my.termios . c.cflag = B9600 | CSS | CREAD | CLCX^AL | HUPCL; 

cfsetospeed(&my.termios , B9600 ); 
t c s e t a 11 r (fd , TCSANOW, &my .termios ); 

} // end if 

return fd ; 

} // end K^OpenSerialPort 

Function: K-CloseSerialPort 


92 



Description: Checks to see if port is open and then closes it. Returns 

port attributes to original configuration . 

Parameter: None 

Return Value: None 

void K_CloseSerialPort() 

{ 

// you may want to restore the saved port attributes 
if (fd > 0) 

{ 

tcsetattr (fd , TCSANOW, &oldtio); 
close(fd); 

} // end if 

} // end K_CloseSerialPort 

Function : K .Write to SerialPort 

Description : 

Parameter: 1} 

Return Value : 

int K.WritetoSerialPort ( unsigned char* psOutput) 

{ 

int iOut ; 

if (fd < 1) // Port is not open, return —I 

{ 

return —1; 

} // end if 

iOut = write(fd, psOutput, 1); // Set to 1 so only one byte is xmitted 

if (iOut < 0) 

{ // Place in ROS.INFO statement!!!!!!! 

//printfi” write error %d%s\n”, errno , str error ( errno )); 

} 

return iOut ; 

} // end K .Write to S e rialPo rt 

Function : K.ReadfromSerialPort 
Description : 

Parameter: I) 
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Return Value: 


/* int K-ReadfromSerial Port ( i n t8-t * psResponse , int iMax) 

{ 

int iln ; 

printfi” in ReadAdrPort iMax=%d\n”, iMax); 

if (fd < 1) 

{ 

printf(” port is not open\n”); 
return —1; 

} // end if 

strncpy (psResponse, ”N/A”, iMax<4?iMax: 4); 
iln = readifd, psResponse , iMax — 1); 
if ( iln < 0) 

{ 

if (errno == EAGAIN) 

{ 

return 0; // assume that command generated no response 

} 

else 

{ 

printfi" read error %d %s\n”, errno, strerror ( errno )); 

} // end if 

} 

else 

{ 

psResponse [ iln <iMax ? iln : iMax] = ’\0\‘ 

printf(”read Vcd chars: %s\n", iln, psResponse); 

} // end if 

return iln ; 

} // end ReadAdrPort 
*/ 


Title : MONTe.Serial.Lib 0.0 

Author: Jason Hickle 

Purpose: List of functions to write serial data for use with MONTe on Linux 

Use: Include ” MONTe .Serial .Lib . h” in the header of each node needed. 

Include source file as part of CMakelist. txt as follows: 
rosbuild.add.executable (node .name src / node .name . cpp src / MONTe .Serial .Lib . cpp) 

Verify that serial handle (/dev/ttySO) is correct for system. 

Version History: 

- Version 0.0 - 
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Mar 21st , 2011 
LT Jason Hickle 


Open and Close Serial port. Write data to serial port. 


/* 

Libraries 

*/ 

#inelude 

< s t dio.h> 


#inelude 

<unistd .h> 


#inelude 

<sys/types.h> 


#inelude 

<sys/stat .h> 


#inelude 

< f c n 11 . h> 


#inelude 

<termios .h> 


#inelude 

< s t r i n g . h> 


#inelude 

<errno . h> 


#inelude 

”MONTe_SeriaI_Lib .h” 


/* 

D efi nes 

*/ 

/* 

Global Variables 

*/ 

static int fd = 0; 


static struct termios oldtio ; 


/* 

Functions 

*/ 


Function: K _OpenSerialPort 

Description: Takes port number and opens appropriate serial connection. 

Will save old port data 

Parameter: I) sPortNumber — pointer to comm port ttyS(X}, ie 0 for ttySO 

etc . 

Return Value: fd — file descriptor for port 

int K_OpenSerialPort(char* sPortNumber) 

{ 

char sPortName [64] = ”/dev/ttySO”; // Hardcoded until generic method works 

// sprintf (sPortName , ”/dev /1 ty S%c ”, ^sPortNumber); // Not working, need to research 

// make sure port is closed 
K.CloseSerialPort (); 

fd = open (sPortName , 0_RDWR | O-NOCITY | O^ELAY); 
if (fd < 0) 

{ 

printf(”open error %d %s\n”, errno, strerror(errno)); 

} 

else 

{ 

struct termios my.termios ; 
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t c g e t a 11 r (fd , &my _termios ); 


oldtio = my.termios ; // Save port attributes to restore later 

tcflush(fd, TCIFLUSH); 

my.termios . c.cflag = B9600 | CSS | CREAD | CLCX^AL | HUPCL; 

cfsetospeed(&my_termios , B9600 ); 
t c s e t a 11 r (fd , TCSANOW, &my .termios ); 

} // end if 

return fd ; 

} // end K^OpenSerialPort 

Function: K^CloseSerialPort 

Description: Checks to see if port is open and then closes it. Returns 

port attributes to original configuration . 

Parameter: None 

Return Value: None 

void K.CloseSerialPortO 

{ 

// you may want to restore the saved port attributes 
if (fd > 0) 

{ 

tcsetattr (fd , TCSANOW, &oldtio); 
close(fd ); 

} // end if 

} // end K_CloseSerialPort 

Function : K.WritetoSerialPort 

Description : 

Parameter: I} 

Return Value: 

int K.WritetoSerialPort ( unsigned char* psOutput) 

{ 

int iOut ; 

if (fd < 1) // Port is not open, return —1 

{ 

return —1; 

} // end if 
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iOut = write(fd, psOutput , 1); 


// Set to 1 so only one byte is xmitted 


if (iOut < 0) 

{ // Place in ROSJNFO statement!!!!!!! 

//printfi” write error %d%s\n”, errno , strerror(errno)); 

} 


return iOut ; 

} // end K .Write to S e rialPo rt 

Function : K.ReadfromSerialPort 
Description : 

Parameter: I) 

Return Value: 

/* int K.ReadfromSerialPort ( intS _t * psResponse , int iMax) 

{ 

int iln ; 

printf(” in ReadAdrPort iMax=%d\n”, iMax); 

if (fd < 1) 

{ 

printfi” port is not open\n”); 
return —1; 

} // end if 

strncpy (psResponse, ”N/A”, iMax<4?iMax: 4); 
iln = readffd, psResponse , iMax — 1); 
if ( iln < 0) 

{ 

if (errno == EAGAIN) 

{ 

return 0; // assume that command generated no response 

} 

else 

{ 

printfC read error %d %s\n”, errno, strerror ( errno )); 

} // end if 

} 

else 

{ 

psResponse [ iln<iMax? iln : iMax] = ’\(9 

printfi” read ^cd chars: %s\n”, iln, psResponse); 

} // end if 

return iln ; 

} // end ReadAdrPort 
*/ 


97 




C.7 Keyboard Control Node 

Following code performs manual eontrol of MONTe. 


Title: MONTe_Keyboard_Control 0.5 (ROS Node) 

Author: Jason Hickle 

Purpose: Node that allows MONTe to be controlled via keyboard over a VNC 

server . 

Handles the following functions : 

1) Take keyboard commands 

2) Change the speed and turning rates 

3) Publishes manual commands to ROS topic 

4) Updates command flags to manual control upon command 

Use: Use the arrow keys to move. P stops the robot. U prompts user to 

change speed. 1 prompts user to change turn rate. Works with 
simp I ifi ed serial mode. 


ROS Notes: 


Name- 

” MONTe .Keyboard.Control ” 

Publications — 

” Plant .Commands .T” 

” Command.Flags.T” 

Subscriptions — 

None 

Messages— 

Plant .Command. msg 

Command.Flags. msg 

Services — 

None 


Version History 


- Version 0.5 

— 

Apr 26th , 2011 

LT Jason Hickle 



Added publishing to ” Command.Flags .T” to update when receiving manual commands 

- Version 0.4 - 

Apr 26th , 2011 
LT Jason Hickle 

Using simplified serial command for motors and utilizing Plant .Command 
msg format. Allow user to change fwd/rev speed and turning rate. 
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- Version 0.3 - 

Apr 26th , 2011 
LT Jason Hickle 

Using packetized serial command for motors and utilizing Manual_Command_2 
msg format. Allow user to change speed and turning rate. 

- Version 0.2 - 

Apr 10th , 2011 
LT Jason Hickle 

Changed commands to simplified serial format from prior packetized 
serial format. Sends new msg format to ” ManuaUCommands ". Return to 
ManuaLCommand. msg vice ManuaUCommand^Simplified. msg if running in 
packetizcd serial mode. 

- Version 0.1 - 

Mar 31st , 2011 
LT Jason Hickle 

Added debug options to verify correct commands. Fixed incorrect keycodes. 

- Version 0.0 - 

Mar 31st , 2011 
LT Jason Hickle 

Enable keyboard commands to send manual control signals over to 
topic Manual_Commands_T for packetized serial mode. 

Notes: Further information can be found on ROS Wiki page: 

http : //www. ros . org / wiki / 

/* Libraries * / 

#include <ros/ros.h> 

#include <signal.h> 

#include <termios.h> 

#include <stdio.h> 

#include <stdlib.h> 

# include ”MONTe/ Plant .Command . h” 

#include ”MONTe/Command_Flags . h” 

/* Debugging */ 

#define COMMAND.VALUEJ^RINT 


/* 

D efi nes 


#define 

KEYCODEJUGHT 

0x43 

#define 

KBYGOnp. T EFT 

0x44 

#define 

KEYCODE_UP 

0x41 

#define 

KEYCODEXOWN 

0x42 

#define 

KEYCODE_Q 

0x71 
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# d e f i n e KEYCODE_SPACE 0x20 


# d efi n e KEYCODE_U 0x75 

# d e f i n e KEYCODEJ 0x69 

# d efi n e KEYCODE_Y 0x79 

#define KEYCODEJ 0x6a 

#define KEYCODE_H 0x68 

const unsigned char STOP_R = 190; 

const unsigned char STOP_L = 64; 

/* Global Variables */ 

// Keyboard control variables 
int kfd = 0; 

struct termios cooked, raw; 

// Command variables 

unsigned char fwd_spd = 30; // Value for going forward 

unsigned char rev.spd = 20; // Value for going reverse 

unsigned char turn.spd = 10; // Differential turning speed 

unsigned char temp.fwd.spd = 30; // temporary storage variables 

unsigned char temp.rev.spd = 20; 
unsigned char temp.turn.spd = 20; 

unsigned char temp.l.turn = 64; // Temp vars for computing turning rates. 

unsigned char temp.r.turn = 190; 
unsigned char temp.turn = 10; 

unsigned char *fwd_spd_ptr , *rev_spd_ptr , * turn.spd.ptr , * temp_l_turn_ptr , * temp_r_turn_ptr ; 

// Pointers for code optimization 

unsigned char L_Correction = 3; // Calibration coefficients 

unsigned char R_Correction = 1; 

unsigned char temp_1 .correct , temp_r_correct ; 

unsigned char * l_correct_ptr , * r _correct _ptr ; 

unsigned char turn.flag = 0; 

/* Functional Prototypes */ 

void quit ( int sig ); 

unsigned char turn .simplified ( unsigned char speed, char flag); 

int main(int argc , char **argv) 

{ 

// Initializations 

// Perform initializations for ROS 

ros :: i n i t ( argc , argv , ’’MONTe.Keyboard.Control” ); // Set up ROS node 

ros :: NodeHandle n; // set up handle for this node 
// Set up all publications for node 

ros :: Publisher man_cmd_pub = n . adverti se <MONTe: : Plant.Command >(”Plant_Command_T” , 1); 
ros :: Publisher cmd_flg_pub = n . adverti se <MONTe: : Command.Flags >(”Command_Flags_T” , 1); 

MONTe:: Plant-Command cmd; 


100 



MONTe:: Command_Flags flags; 

// End ROS initialization 

/* Variable and pointer initialization */ 

cmd. left = STOP_L; 
cmd. right = STOP_R; 
fwd.spd.ptr = &fwd_spd ; 
rev.spd.ptr = &rev_spd ; 
turn.spd.ptr = &turn_spd; 
temp_l_turn_ptr = &temp_l_turn ; 
temp_r_turn_ptr = &temp_r_turn ; 
l.correct.ptr = &L_Correction ; 
r_correct_ptr = &R_Correction ; 

// Set the auto^nav flag to 0 (manual) when manual command is 
flags . auto_nav = 0; 

signal (SIGINT , quit ); 

// Initialize keyboard 

// get the console in raw mode 
tcgeta11r (kfd , &cooked); 

memcpy(&raw , &cooked , sizeof ( struct termios)); 
raw.c.lflag &=~ (ICANON | ECHO); 

// Setting a new line , then end of file 
raw . c_cc [VEOL] = 1; 
raw . c_cc [VEOF] = 2; 
tcsetattr (kfd , TCSANOW, &raw ); 


std 

: cout 

« 

’’Reading from keyboard\n”; 

std 

: cout 

« 


— 


\n ’; 

std 

: cout 

« 

’’Use 

arrow keys 

to move MONTe.\ n” ; 

std 

: cout 

« 

”P 

stops MONTe.\ 

n” ; 

std 

: cout 

« 

”Y 

to 

enter new 

reverse speed.\n”; 

std 

: cout 

« 

”U 

to 

enter new 

reverse speed.\n”; 

std 

: cout 

« 

”I 

to 

enter new 

turn rate.\n”; 

std 

: cout 

« 

”H 

to 

enter new 

left wheg correction .\n” ; 

std 

: cout 

« 


to 

enter new 

right wheg correction .\n” ; 

std 

: cout 

« 

’’Current settings: Fwd speed ” « (int) fwd.spd « 


« (int) rev.spd « ” Turn rate ” « (int) turn.spd « 
//puts(”Q quits ROS.’'); 


while ( ros : : ok ()) 

{ 

char c; 

bool dirty = false; 

// get the next event from the keyboard 
if(read(kfd, &c, 1) < 0) 

{ 

perror(”read ():” ); 


received 


” Rev speed 

\n”; 
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exit (— 1); 

} 

ROSJ)EBUG( ’’value : 0x%02X\n” , c ); 

switch (c) 

{ 

case KEYCODEXEFT: // Turn left 
ROS_DEBUG( ’’LEFT” ); 

// Computes turn differential speed for whegs and adds correction factor 

* temp_l_turn_ptr = turn.simplified (STOP_L + fwd.spd , 0); 

* temp_r_turn_ptr = turn.simplified (STOP_R + fwd.spd, 1); 

cmd.left = *temp.l.turn.ptr; 

cmd. right = *temp.r.turn.ptr; 

dirty = true ; 

ROS_INFO(”Going Left\n”); 

break; 

case KEYCODEJUGHT: // Turn right 
ROSJ)EBUG( ’’RIGHT” ); 

// Computes turn differential speed for whegs and adds correction factor 

* temp_l_turn_ptr = turn.simplified (STOP_L + fwd.spd, 1); 

* temp.r.turn.ptr = turn.simplified (STOP.R + fwd.spd, 0); 

cmd.left = *temp.l.turn.ptr; 

cmd. right = *temp.r.turn.ptr; 

dirty = true ; 

ROS_INEO(”Going Right\n”); 

break; 

case KEYCODE_UP: // Go forward 

ROSJDEBUG( ”UP” ); 

// Enters desired fwd speed and adds calibration correction 
cmd.left = STOP.L + *fwd.spd.ptr + * 1 .c o r r e c t. p tr ; 
cmd. right = STOP.R + *fwd.spd.ptr + * r.correct.ptr ; 

dirty = true ; 

ROS_INEO(”Going Eorward\n” ); 

break; 

case KRYCODEDOWN: // Go in reverse 

ROSJDEBUG( ”DOWN” ); 

// Enters desired rev speed and applies calibration correction 
cmd .left = STOP.L — *rev.spd.ptr — *1.correct.ptr; 
cmd. right = STOP.R — *rev.spd.ptr — *r.correct.ptr; 

dirty = true ; 
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ROS_INFO( ’’Going Back wards \n” ); 


break; 

case KEYCODE_SPACE: // Stop 

ROS_DEBUG( ’’STOP” ); 

cmd. left = STOP_L; 
cmd. right = STOP_R; 

dirty = true ; 

ROS_INFO( ” Stopping\n” ); 

break; 

case KEYCODE_Y: // Enter new forward speed 

ROS_DEBUG(”Enter new forward speed”); 

do { // User inputs speed and checks for valid input 

std : : cout « ”\nEnter forward speed (0—50).”; 
scanf(”%d”, &temp_fwd_spd ); 

} while (temp.fwd.spd > 50); 

fwd.spd = temp.fwd.spd ; 

dirty = true ; 

ROS_INFO(”New forward speed %d\n” , fwd.spd); 
continue ; // Return to top of while loop 

case KEYCODE.U: // Enter new reverse speed 

ROSJDEBUG(”Enter new reverse speed”); 

do { // User inputs speed and checks for valid input 

printf (”\nEnter new reverse speed (0—50).”); 
scanf(”%d”, &temp.rev.spd ); 

} while (temp.rev.spd > 50); 

rev.spd = temp.rev.spd ; 

dirty = true ; 

ROS.INFO(’’New reverse speed %d\n” , rev.spd); 
continue ; // Return to top of while loop 

case KEYCODEJ: // Enter new turn rate 
ROS_DEBUG( ” Enter turn rate”); 

do { // User inputs speed and checks for valid input 

printf (”\nEnter turn rate (0 — 40).”); 
scanf(”%d”, &temp.turn.spd ); 

} while (temp.turn.spd > 40); 
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turn.spd = temp.turn.spd ; 


dirty = true ; 

ROS_INFO(”New turn rate %d\n” , turn.spd); 
continue; // return to top of while loop 

case KEYCODEJt: // Enter correction factor for left set of whegs 
ROSJDEBUG( ” Enter left speed correction”); 

do { // User inputs speed and checks for valid input 

printf (”\nEnter left correction factor (0 — 13).”); 
scanf(”%d”, &temp_l_correct); 

} while ( temp.l.correct > 13); 

* 1 .correct.ptr = temp.l.correct; 
dirty = true ; 

ROS_INFO(”New left correction %d\n” , * 1 _c o r r e c t _p t r ); 
continue ; // return to top of while loop 

case KEYCODEJ: // Enter correction factor for right set of whegs 
ROSJDEBUG( ” Enter left speed correction”); 

do {// User inputs speed and checks for valid input 

printf (”\nEnter right correction factor (0 — 13).”); 
scanf(”%d”, &temp_r_correct); 

} while ( temp.r.correct > 13); 

* r _c o r r e c t _p t r = temp.r.correct; 
dirty = true ; 

ROS_INFO (’’New left correction %d\n” , * r _ c o r r e c t _p tr ); 
continue; // return to top of while loop 


default : 

continue; // return to top of while loop 

} // end switch 
#ifdef COMMAND.VALUEJ^RINT 

ROS.INFO( ” Speed : Left %d\tRight %d” , cmd.left , cmd. right); 

#endif 


man.cmd.pub . publish (cmd); 
cmd.flg.pub . publish (flags ); 

dirty = false ; 

} // End main while loop 
} //end main 
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Function: quit 


Description : 

Parameter: I) 

Return Value : 

void quit ( int sig ) 

{ 

tc setattr (kfd , TCSANOW, &cooked); 
ros : : shutdown (); 
exit (0); 

} 


Function: turn .simplified 

Description: Takes turn signals and outputs proper simplified serial value 

Parameter: 1) Forward speed 

2) Inside whegs (0) , outside whegs (1) 

Return Value: Simplified serial value (64 — 127 for left whegs, 190—255 for right 

whegs }. 

unsigned char turn .simplified (unsigned char speed, char flag) 

{ 

unsigned char final.speed; 
if(speed < 64) 

return 0; // Bad forward turn command, sends a 0 which will 

// stop the motor 

if(speed >= 190) 

{ if (flag == 0)// For the right motor, inside track, ensures min turn speed is 190 (stop) 

final.speed = ((speed — * turn.spd.ptr + * r.correct.ptr) >= 190) ? 

(speed — *turn.spd.ptr + *r.correct.ptr) : 190; 

else if(flag == 1)// If right side it outside turn, max turn spd at 255 (full fwd) 

final.speed = ((speed + * turn.spd.ptr + * r.correct.ptr) <= 255) ? 

(speed + *turn.spd.ptr + *r.correct.ptr) : 255; 

} else // Left motor command, min turn speed is 64( stop) and 127 (full fwd) 

{ if (flag == 0)// For the right motor, inside track, ensures min turn speed is 64 (stop) 

final.speed = ((speed — * turn.spd.ptr + * 1. c o r r e c t .p t r ) >= 64) ? 

(speed — * turn.spd.ptr + * 1.correct.ptr) : 64; 
else if(flag == 1)// If right side it outside turn, max turn spd at 127 (full fwd) 

final.speed = ((speed + * turn.spd.ptr + * 1. c o r r e c t .p t r ) <= 127) ? 

(speed + * turn.spd.ptr + * 1.correct.ptr) : 127; 

} // end parsing if 
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return final_speed; 
} // end turn -Simplified 


C.8 Waypoint Control Node 


Following code performs manual eontrol of MONTe. 


Title: MONTe_Waypoint_Control 0.0 (ROS Node) 

Author: Jason Hickle 


Purpose: Node that allows user to input waypoints over VNC server. 

Handles the following functions : 

1) Add waypoints to queue. 

2) Send route to New-Waypoints 

3) Delete all waypoints 

4) Update Command-Flags when in auto—nav. 

Use: Rudimentary user input program. Inputs waypoints and then sends 

them to Waypoint-Processing via New-Waypoint-T. Route is sent at 4Hz 
to ensure waypoints are not lost. This synchs up with program rate. 

Utilizes class structure for manipulating route. 

Actions not used in current code, but kept for future use. Could be 
used to indicate search routines , return to base, etc. Current 
behavior is to stop after completion of last waypoint. 

Next step is to take the class structure further and subsume 
ROS initilization s into the private portion. This will tranisition 
to a more object-oriented format than currently implemented. 


ROS Notes: 

Name— ” MONTe -Waypoint-Control 

Publications — ” N ew -Waypoints -T” 

” Command-Flags-T” 


Subscriptions — None 


Messages— Waypoint, msg 

Command-Flags. msg 


Services — None 


Version History: 

- Version 0.0 - 

May 19th, 2011 
LT Jason Hickle 
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Allow input, deletion and sending of new waypoints. 


Notes: Further information can be found on ROS Wiki page: 

http : //www. ros . org / wiki / 

/* Libraries * / 

#include <ros/ros.h> 

#include <signal.h> 

#iiiclude <termios.h> 

#include <stdio.h> 

#include <stdlib.h> 

#include ’’MONTe/Command_Flags . h” // MONTe messages 

#include ’’MONTe/Waypoint. h” 

/* D efi nes */ 

// Debugging options , uncomment to enable 

#define WP_PRINT // Print statement for new waypoints 

#define KEYCODEJD 0x64 // For user inputs 

#define KEYCODE_N 0x6e 

#define KEYCODE_S 0x73 

/* WP stores all data necessary to navigate to, and determine / 

/ behavior mode for a route. */ 

typedef struct { 

int number ; 
double latitude ; 
double longitude ; 
char action ; 

} WP; 

/* class Waypoints provides methods for manipulating a waypoint route */ 

class Waypoints { 
private : 

public : 

struct { 

int number ; 
double latitude ; 
double longitude ; 
char action ; 

} wp[10]; 

int wp_count; 
bool wp.entered ; 

Waypoints(){ // Constructor for class Waypoints 

wp[0]. number = 0; 
wp.count = 0; 
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wp_entered = 0; // Ensures route not sent until wp is entered 

} 


void Add-Waypoint 0 ; 
void Delete.Route 0 ; 

}; 

void Waypoints :: Add.Waypoint 0 

{ 

double temp.lat , temp.lon ; 
char temp.action ; 

wp[ wp_count ]. number = wp_count; 

printf (”\nPlease enter waypoint %d latitude (dec degrees, N is positive): 
scanf(”%lf”, &temp_lat); 

wp[ wp_count ]. latitude = temp_lat; 

printf (”\nPlease enter waypoint %d longitude (dec degrees , E is positive): 
scanf(”%lf”, &temp_lon); 

wpt wp.count ]. longitude = temp.lon; 

// Need to get better i/o option for following 
/* //do { 

printf(”\nPlease enter waypoint %d action (a—continue, s—stop): ", 
scanf(”%c", &temp .action ); 

//} while ((temp.action != ’a’) || (temp.action != ’s ’)); 

wp [ wp .count ]. action = temp.action; */ 


#ifdef WP_PRINT 


ROS_INFO(”Wp #%d, Latitude - %lf , Longitude = %lf ” , 

wpt wp.count]. longitude ); 


#endif 


wp.count++; 
wp.entered = 1; 


wp.count, wpt wp.count ]. latitude 


void Waypoints :: Delete.Route 0 // Recursive function to delete stored wp data 

{ 

if (wp.count > 9) // Prevents out of out of bounds 

wp.count = 9; 

while (wp.count > 0) // Deletes all data until wp.count=0 

{ 

wp[ wp.count ]. number = 0; // Null all data 

wp[ wp.count ]. latitude = 0.0; 
wp[ wp.count ]. longitude = 0.0; 
wp[ wp.count ]. action = NULL; 


, wp.count); 


, wp.count) 


wp.count); 
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wp_count-; // Decrement wp-count for next recursion 

Waypoints :: Delete.Route (); // Call Delete-Route again to delete next wp 


} 


} 

if(wp_count == 0) // Check in case of bad data 

{ 

wp[ wp.count ]. number = 0; // Null all data 

wp[ wp.count ]. latitude = 0.0; 
wp[ wp.count ]. longitude = 0.0; 
wp[ wp.count ]. action = NULL; 

wp.entered = 0; // Reset flag to indicate no stored waypoints 


/* Global Variables */ 

// Keyboard control variables 
int kfd = 0; 

struct termios cooked, raw; 
int counter ; 

/* Functional Prototypes */ 

void Instructions (); 

void quit ( int sig ); 

int main(int argc , char **argv) 

{ 

/* Initializations */ 

// Perform initializations for ROS 

ros :: i n i t ( argc , argv , ”MONTe_Waypoint_Control” ); // Set up ROS node 

ros :: NodeHandle n; // set up handle for this node 

ros:: Rate loop.rate (4); // Sets 4hz cycle for main loop 

// Set up all publications for node 

ros :: Publisher cmd.flg.pub = n . advertise <MONTe:: Command.Flags >(”Command.Flags.T” , 1) 

ros :: Publisher new.wp.pub = n . adverti se <MONTe:: Waypoint >(”New.Waypoint.T” , 10); 

MONTe:: Command.Flags flags; 

MONTe:: Waypoint new.wp ; 

signal (SIGINT , quit ); 

Waypoints Waypoint.Queue ; 

/* Initialize keyboard for user input */ 

// get the console in raw mode 
tcgeta11r (kfd , &cooked); 

memcpy(&raw , &cooked , sizeof ( struct termios)); 


109 



raw.c_lflag &=' (ICANON | ECHO); 


// Setting a new line , then end of file 
raw . c_cc [VEOL] = 1; 
raw . c_cc [VEOF] = 2; 
t c s e t a 11 r (kfd , TCSANOW, &raw ); 
while ( ros : : ok ()) 

{ 

char input.cmd ; 

Instructions (); // Print instructions at each loop 

// get the next event from the keyboard 
if(read(kfd, &input_cmd , 1) < 0) 

{ 

perror(”read ():” ); 
exit ( — 1); 

} 


switch (input.cmd) 

{ 

case KEYCODEJSF: // Enter new waypoint 

i f (Waypoint.Queue . wp.count < 10) 

Way point .Queue. Add .Waypoint (); 
else 


break; 


ROS.INFO(’’Max waypoints reached!”); 


case KEYCODE_S; // Send new route 

if (( Waypoint.Queue . wp.entered == 1) && (Waypoint.Queue . wp.count > 0)) 

{ 

counter = 0; // Send waypoints 


// Warn Waypointrocessing new route incoming 
flags . auto.nav = 0; 
flags . nav.mode = ’A’; 

// Navigation takes over nav-mode 
flags . incoming.route = 1; 

// Indicate new route to system 
cmd.flg.pub . publish (flags ); 

loop.rate . sleep 0 ; // Sleeps to maintain loop-rate 

// Enter send waypoint loop, will send messages until all wp sent 
while ( counter < Waypoint.Queue . wp.count) 

{ 

new.wp . latitude = Waypoint.Queue .wp[ counter ]. latitude ; 

new.wp . longitude = Waypoint.Queue .wp[ counter ]. longitude 

new.wp. action = ’a’; // stop and wait 

new.wp . wp.num = counter; 

new.wp. route = Waypoint.Queue . wp.count; 
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new_wp_pub . publish (new_wp ); 

// Publish to ROS Topic 

#ifdef WP_PRINT 

ROS_INFO(”Waypoint %d sent!”, counter); 
#endif 

flags . auto.nav = 1; 

//Place MONTe in autonomous navigation 
if ( counter == 0) 

flags . nav.mode = ’N’; 

//Tell Waypoint_Processing to send first waypoint 
// for first waypoint sent 

else 

f 1 ag s . nav_mode = ’A’; 
//Navigation takes over nav^mode 
flags . incoming_route = 1; 

//Indicate new route to system 
cmd.flg.pub . publish (flags ); 
counter ++; 
loop.rate . sleep (); 

// Sleeps to maintain loop _rate 
} // End send waypoint loop 
} else 

{ 

ROS_INFO(”No route in queue to send.”); 

continue ; 

} 


ROS-INFO (’’MONTe is in autonomous navigation”); 

break; 

case KEYCODEX): // Delete route 

if ( Waypoint-Queue . wp.entered == 1) 

Waypoint.Queue.Delete.Route (); 
else 

ROS_INFO(”No waypoints in queue to delete.” 


break; 


default : 

continue ; 

} // end switch 

} // end main while loop 

} //end main 

Function: Instructions 


111 



Description : 


Print out instructions for node 


Parameter: None 

Return Value: None 

void Instructions 0 

{ 

printf (”\nWelcome to MONTe waypoint control ...\n”); 

printf (’’Please enter :\n\t\tn — Enter new waypoint\n\t\td — Delete route\n”); 
printf(”\t\ts — Send route\n”); 

} // End Instructions 

Function: quit 

Description : 

Parameter: 1) 

Return Value: 

void quit ( int sig ) 

{ 

tc setattr (kfd , TCSANOW, &cooked); 
ros : : shutdown (); 
exit (0); 
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APPENDIX D: 
ROS Messages 


The following are the message formats used by MONTe. 

# All messages for MONTe 

# Used for sending information between topics 

# Command.Flags . msg 

# Contains all behavior flags for MONTe. 

# Autonomous Navigation flag : 1 for autonav , 0 for manual control 

bool auto.nav 

# Navigation Mode: A — Enroute to current Waypoint, N— Current waypoint reached 
char nav_mode 

# Route flag indicates new set of waypoints 
bool incoming-route 

# Nav-Data . msg 

# Send a waypoint for MONTe. 

# Latitude in decimal degrees . 
float64 latitude 

# Longitude in decimal minutes 
float64 longitude 

# Heading 
float64 heading 

# Action character 
char action 

# Plant-Command . msg 

# 

# Basic message for manually controlling MONTe in simplified serial mode. 

# Speed command for left motor. Range is l(Full Reverse)—> 64 (Stop) <— 127 (Full Forward) 

Li i n 18 left 

# Speed command for left motor. Range is 128(Full Reverse)—> 192 (Stop) <— 255 (Full Forward) 
uintS right 
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# Waypoint. msg 

# 

# Send a waypoint for MONTe. 

# Latitude in decimal degrees . 
float64 latitude 

# Longitude in decimal minutes 
float64 longitude 

# Action character 
char action 

# Waypoint number (0 — 9) 
intS wp_num 

# Number of waypoints in route (New_Waypoint only) 
intS route 
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